def aruco_read(img_copy):
"""
Função que processa uma imagem, buscando os QR codes.
Retorna:
img_copy: copia da imagem da camera do robo, com
o contorno dos QRcodes desenhados por cima, e com os IDs dos
QRcodes sobrescritos na imagem.
ids: lista dos IDs dos QRcodes, que a funcao esta
detectando na imagem.
"""
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
# parameters = aruco.DetectorParameters_create()
# parameters.minDistanceToBorder = 0
# parameters.adaptiveThreshWinSizeMax = 1000
gray = cv2.cvtColor(img_copy, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict) #, parameters=parameters)
# if ids is None:
# return None
# for i in range(len(ids)):
# print('ID: {}'.format(ids[i]))
# for c in corners[i]:
# for canto in c:
# print("Corner {}".format(canto))
aruco.drawDetectedMarkers(img_copy, corners, ids)
return img_copy, ids
def detect_Aruco(
img): #returns the detected aruco list dictionary with id: corners
aruco_list = {}
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.getPredefinedDictionary(
aruco.DICT_4X4_1000
) #aruco.Dictionary_get(aruco.DICT_4X4_50) #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249
print(aruco_dict)
parameters = aruco.DetectorParameters_create(
) #refer opencv page for clarification
#lists of ids and the corners beloning to each id
print(parameters)
corners, ids, _ = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
#corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left.
# print len(corners), corners, ids
print(corners)
print(len(corners))
gray = aruco.drawDetectedMarkers(gray, corners, ids)
# cv2.imshow('frame',gray)
#print (type(corners[0]))
if len(corners): #returns no of arucos
#print (len(corners))
#print (len(ids))
for k in range(len(corners)):
temp_1 = corners[k]
temp_1 = temp_1[0]
temp_2 = ids[k]
temp_2 = temp_2[0]
aruco_list[temp_2] = temp_1
return aruco_list
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 __init__(self):
global take_off
# --- Get the camera calibration path
self.twist = Twist()
self.workspace_path = '/home/vishal/vishal_testing'
self.camera_matrix = np.loadtxt(
self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraMatrix.txt', delimiter=',')
self.camera_distortion = np.loadtxt(
self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraDistortion.txt', delimiter=',')
# --- 180 deg rotation matrix around the x axis
self.r_flip = np.zeros((3, 3), dtype=np.float32)
self.r_flip[0, 0] = 1.0
self.r_flip[1, 1] = -1.0
self.r_flip[2, 2] = -1.0
# --- Define the aruco dictionary
self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
self.parameters = aruco.DetectorParameters_create()
# --- Capture the camera video (this may also be a video or a picture)
self.capture = cv_bridge.CvBridge()
# -- Font for the text in the image
self.font = cv2.FONT_HERSHEY_PLAIN
self.height = 0
self.downward = 0
self.upword = 0
self.height_sub = rospy.Subscriber('/sonar_height', Range, callback=self.height_callback)
self.camera_image_sub = rospy.Subscriber('/bottom/camera/image', Image, queue_size=5, callback=self.main)
self.drone_status_pub = rospy.Publisher('land', String, queue_size=1)
self.velocity_command_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
def startWebCam(cameraMatrix, distCoeffs,arucoSquareDimension):
markerCorners=[]
#markerCorners=[]
markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
cap = cv2.VideoCapture(cam_1_no)
while(1):
_,frame = cap.read()
frame,new_matrix_1,_=undistortImage(frame,cam_matrix_1,distortion_coefficients_1)
markerCorners,markerID, rejectedCandidates= aruco.detectMarkers(frame,markerDictionary)
r_vect,t_vect, _ =aruco.estimatePoseSingleMarkers(markerCorners,arucoSquareDimension,cameraMatrix,distCoeffs)
if(markerID is not None):
for i in range(len(markerID)):
#aruco.draw
aruco.drawAxis(frame,cameraMatrix,distCoeffs,r_vect[i],t_vect[i],0.1)
aruco.drawDetectedMarkers(frame,markerCorners)
# print("Corners",markerCorners[0][0])
#focalLength = (abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0]) * KNOWN_DISTANCE) / KNOWN_WIDTH
#print("Focal length: ",focalLength)
print("Length: ",distance_to_camera(KNOWN_WIDTH,FOCAL,abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0])))
cv2.imshow('Webcam',frame)
if(cv2.waitKey(30)>=0):
break
return 1
def __init__(self,
squaresX,
squaresY,
square_length,
marker_length,
marker_bits=4,
dict_size=50,
aruco_dict=None,
manually_verify=False):
self.squaresX = squaresX
self.squaresY = squaresY
self.square_length = square_length
self.marker_length = marker_length
self.manually_verify = manually_verify
dkey = (marker_bits, dict_size)
self.dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])
self.board = aruco.CharucoBoard_create(squaresX, squaresY,
square_length, marker_length,
self.dictionary)
total_size = (squaresX - 1) * (squaresY - 1)
objp = np.zeros((total_size, 3), np.float64)
objp[:, :2] = np.mgrid[0:(squaresX - 1),
0:(squaresY - 1)].T.reshape(-1, 2)
objp *= square_length
self.objPoints = objp
self.empty_detection = np.zeros((total_size, 1, 2)) * np.nan
self.total_size = total_size
def identifica_id(cv_image):
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# --- Define the aruco dictionary
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
# parameters = aruco.DetectorParameters_create()
# parameters.minDistanceToBorder = 0
# parameters.adaptiveThreshWinSizeMax = 1000
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict) # , parameters=parameters)
aruco.drawDetectedMarkers(cv_image, corners, ids)
if ids is None:
ids = [[0]]
centro = (0, 0)
else:
ids = ids.tolist()
corners = corners[0][0]
media_x = int(
(corners[0][0] + corners[1][0] + corners[2][0] + corners[3][0]) /
4)
media_y = int(
(corners[0][1] + corners[1][1] + corners[2][1] + corners[3][1]) /
4)
centro = (media_x, media_y)
return centro, ids[0][0], ids[0]
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 __init__(self, marker_size, camera_matrix, camera_distortion):
#The CvBridge command is only needed if the images are coming in via ROS
#self.bridge = CvBridge()
self.marker_size = marker_size
self.camera_matrix = camera_matrix
self.camera_distortion = camera_distortion
self.R_flip = np.zeros((3, 3), dtype=np.float32)
self.R_flip[0, 0] = 1.0
self.R_flip[1, 1] = -1.0
self.R_flip[2, 2] = -1.0
#The following commands establish which Aruco dictionary will be in use.
self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_100)
self.parameters = aruco.DetectorParameters_create()
#initialize video stream
self.vid = cv2.VideoCapture(0)
#initialize an empty frame within the class
self.frame = np.zeros([960, 1280, 3], dtype=np.uint8)
self.gray = np.zeros([960, 1280, 3], dtype=np.uint8)
self.font = cv2.FONT_HERSHEY_PLAIN
def create_board(cor, a_id):
board_corners = [np.array(cor, dtype=np.float32)]
board_ids = np.array([[a_id]], dtype=np.int32)
board = aruco.Board_create(
board_corners, aruco.getPredefinedDictionary(aruco.DICT_5X5_250),
board_ids)
return board
def main():
# establish global constants (hardcode in file or make optional param to fn call)
# KNOWN_WIDTH is the width of the AR tags to detect in URC competition
KNOWN_WIDTH = 20 # unit is cm
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))
# allow the camera to warmup
time.sleep(2)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# print("Captured Image")
# grab the raw NumPy array representing the image
image = np.array(frame.array)
# Our operations on the frame come here
gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
#TODO: change "cv2.aruco.DICT_5x5" to rover aruco lib
aruco_dict = aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_50)
parameters = aruco.DetectorParameters_create()
# print("Parameters:", parameters)
# Lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray_img, aruco_dict, parameters=parameters)
# print("Corners:", corners)
# print(type(id))
if (len(corners) > 0):
for i in range(len(corners)): # used to be id
# call function to calculate ar tag heading relative to rover
depth = get_depth(i, corners[i]) #used to be id[i]
heading = global_coord_trans(i, corners[i], depth) # id[i]
# this is where we would send heading over LCM channel
# note: global coord trans could happen somewhere
# outside of this script. It doesn't, but it could
print("Depth and Heading of", i, "are:", depth, "&", heading)
gray_img = aruco.drawDetectedMarkers(gray_img, corners)
#print(rejectedImgPoints)
# Display the resulting frame
cv2.imshow('frame', gray_img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
rawCapture.truncate(0)
# When everything done, release the capture
# cap.release()
cv2.destroyAllWindows()
return
def callback(data):
global theID, thePoint, cam_tf, cam_pub
####for real tello
#camera_matrix = np.array([[921.170702, 0.000000, 459.904354], [
# 0.000000, 919.018377, 351.238301], [0.000000, 0.000000, 1.000000]])
#dist_coeffs = np.array(
# [-0.033458, 0.105152, 0.001256, -0.006647, 0.000000])
####for sim
camera_matrix = np.array([[562, 0.000000, 480.5], [0.000000, 562, 360.5],
[0.000000, 0.000000, 1.000000]])
dist_coeffs = np.array([0, 0, 0, 0, 0])
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
board_corners = [np.array(thePoint, dtype=np.float32)]
board_ids = np.array([theID], dtype=np.int32)
board = aruco.Board_create(
board_corners,
aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL), board_ids)
i = 0
corners = []
ids = np.array([data.id], dtype=np.int32)
corners = np.array([[[data.points[0].x, data.points[0].y],
[data.points[1].x, data.points[1].y],
[data.points[2].x, data.points[2].y],
[data.points[3].x, data.points[3].y]]],
dtype=np.float32)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_matrix, dist_coeffs,
None, None)
if retval == 0:
return 0, 0, 0
a = rvec
b = tvec
rr, _ = cv2.Rodrigues(np.array([a[0][0], a[1][0], a[2][0]]))
tt = np.array([b[0][0], b[1][0], b[2][0] + 0.21])
cam_r = rr.transpose()
cam_t = -cam_r.dot(tt)
cam_x = cam_t[0]
cam_y = cam_t[1]
cam_z = cam_t[2]
cam_q = quaternion_from_matrix(cam_r)
tq = [-0.5, 0.5, 0.5, 0.5]
q3 = qmulq(tq, cam_q)
cam_tf.sendTransform((cam_x, cam_y, cam_z), cam_q, rospy.Time.now(),
"Tello_cam_from_marker" + str(theID), "world")
cam_tf_t.sendTransform((0, 0, 0), (0.5, -0.5, 0.5, 0.5), rospy.Time.now(),
"Tello_cam_from_marker_world" + str(theID),
"Tello_cam_from_marker" + str(theID))
pubmsg = PoseStamped()
pubmsg.pose.position.x = cam_x
pubmsg.pose.position.y = cam_y
pubmsg.pose.position.z = cam_z
pubmsg.pose.orientation.w = q3[3]
pubmsg.pose.orientation.x = q3[0]
pubmsg.pose.orientation.y = q3[1]
pubmsg.pose.orientation.z = q3[2]
pubmsg.header = data.header
pubmsg.header.frame_id = "world"
cam_pub.publish(pubmsg)
def __init__(self, videoPort, cameraMatrix, distortionCoefficients):
#カメラ読み込み
self.cap = cv2.VideoCapture(videoPort)
#カメラの内部パラメータ読み込み
self.cameraMatrix = np.load(cameraMatrix)
self.distortionCoefficients = np.load(distortionCoefficients)
#arucoの辞書読み込み
self.dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
def __init__(self):
self._cap = cv2.VideoCapture(VIDEO_SOURCE_ID)
self._dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
self._font = cv2.FONT_HERSHEY_SIMPLEX
rospy.Subscriber("/neato01/pose", PoseStamped, self.tracked_neato01, queue_size=10)
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.resizeWindow('frame', 1280, 720)
self._start_time = time.time()
def detect_Aruco(image):
img=image
aruco_list = {}
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_250)
parameters = aruco.DetectorParameters_create()
_, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
return ids
def configure_rigid_bodies(configuration):
"""
reads configuration and creates a list of rigid bodies
together with a list of dictionaries used.
"""
dictionary_name = configuration.get("aruco dictionary", 'DICT_4X4_50')
try:
ar_dictionary_name = getattr(aruco, dictionary_name)
except AttributeError:
raise ImportError(('Failed when trying to import {} from cv2.'
'aruco. Check dictionary exists.'
).format(dictionary_name)) from AttributeError
ar_dicts = []
ar_dict_names = []
ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name))
ar_dict_names.append(dictionary_name)
rigid_bodies = []
rigid_body_configs = configuration.get('rigid bodies', [])
for rigid_body_config in rigid_body_configs:
rigid_body = ArUcoRigidBody(rigid_body_config.get('name', 'no name'))
filename = rigid_body_config.get('filename', None)
if filename is None:
raise ValueError('rigid body configuration must include filename')
dictionary_name = rigid_body_config.get('aruco dictionary',
'DICT_ARUCO_ORIGINAL')
try:
ar_dictionary_name = getattr(aruco, dictionary_name)
except AttributeError:
raise ImportError(('Failed when trying to import {} from cv2.'
'aruco. Check dictionary exists.'
).format(dictionary_name)) from AttributeError
rigid_body.load_3d_points(filename, dictionary_name)
tag_width = rigid_body_config.get('tag width', None)
if tag_width is not None:
rigid_body.scale_3d_tags(tag_width)
rigid_bodies.append(rigid_body)
if dictionary_name not in ar_dict_names:
ar_dict_names.append(dictionary_name)
ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name))
return ar_dicts, ar_dict_names, rigid_bodies
def show_camera(self):
flag, self.image = self.cap.read()
if self.aruco_flag is True:
if self.camera_matrix is None:
error_dialog = QErrorMessage()
error_dialog.showMessage(
'Oh no you do not have load the matrix\n'
'Please sure you have clicked the load mtx button\n'
'Program will close in 1 sec\n')
error_dialog.exec_()
exit()
# load the matrix in Camera Matrix
aruco_dict = aruco.getPredefinedDictionary(
aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()
gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
ret, out1 = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY)
corners, ids, rejected = aruco.detectMarkers(
image=out1,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=self.camera_matrix,
distCoeff=self.camera_distortion)
if ids is not None:
# -- Find the marker's 6-DOF pose
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(
corners, 140, self.camera_matrix, self.camera_distortion)
# print(rvecs)
for i in range(rvecs.shape[0]):
aruco.drawAxis(self.image, self.camera_matrix,
self.camera_distortion, rvecs[i, :, :],
tvecs[i, :, :], 100)
aruco.drawDetectedMarkers(self.image, corners)
id_str = "id:{}".format(ids[0][0])
rvecs_str = "rvecs:Rx: {:.2f},Ry: {:.2f},Rz: {:.2f}".format(
rvecs[0][0][0], rvecs[0][0][1], rvecs[0][0][2])
tvecs_str = "tvecs:Tx: {:.3f},Ty: {:.3f},Tz: {:.3f}".format(
tvecs[0][0][0], tvecs[0][0][1], tvecs[0][0][2])
center_point_str = f"center point {corners}"
# cv2.putText(self.image, id_str, (0, 30), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
# cv2.putText(self.image, rvecs_str, (0, 130), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
# cv2.putText(self.image, tvecs_str, (0, 230), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
# cv2.putText(self.image, center_point_str, (0, 330), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 255, 0), 5)
self.tvec_label.setText(tvecs_str)
self.rvec_label.setText(rvecs_str)
else:
pass
show = cv2.resize(self.image, (1920, 1080))
show = cv2.cvtColor(show, cv2.COLOR_BGR2RGB)
showImage = QImage(show.data, show.shape[1], show.shape[0],
QImage.Format_RGB888)
self.label_show_camera.setPixmap(QPixmap.fromImage(showImage))
self.label_show_camera.resize(self.widget.size())
self.label_show_camera.setScaledContents(True)
def createArucoMarkers():
outputMarker = []
markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
for i in range(50):
outputMarker = aruco.drawMarker(markerDictionary, i, 500)
imageName = "4x4Marker_" + str(i) + ".jpg"
#f=open(imageName,"w")
cv2.imwrite(imageName, outputMarker)
def test_get_dictionary(self):
"""
compare public attributes of dictionary to confirm equality
"""
err_msg = "get_dictionary failed"
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
self.assertTrue(
FiducialMarker.dictionary_equal(dictionary,
FiducialMarker.get_dictionary()))
def detecter():
cap = cv2.VideoCapture(2)
Setcamera(cap)
cameraMartix = np.array([[720.2120, 2.0761, 335.1827],
[0, 721.1528, 268.9376], [0, 0, 1]])
distCoeffs = np.array([-0.1622, 0.1400, 0.00234976, -0.0008216553])
dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
pub = rospy.Publisher('visual', Visual_msg, queue_size=10)
rospy.init_node('detecter', anonymous=True)
rate = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
ret, frame = cap.read()
img = frame
corners, ids, reject = aruco.detectMarkers(frame, dict)
if ids is not None:
img = aruco.drawDetectedMarkers(img, corners, ids)
r, t = aruco.estimatePoseSingleMarkers(corners, 0.95, cameraMartix,
distCoeffs)
position = xyz(r, t)
# rospy.loginfo("r:\n",r)
# rospy.loginfo("t:\n",t)
for i, id in enumerate(ids):
img = aruco.drawAxis(img, cameraMartix, distCoeffs, r[i], t[i],
1)
msg = Visual_msg()
msg.id = id
marker_r = r[i][0]
marker_t = t[i][0]
msg.rvec.x = marker_r[0]
msg.rvec.y = marker_r[1]
msg.rvec.z = marker_r[2]
msg.tvec.x = position[0]
msg.tvec.y = position[1]
msg.tvec.z = position[2]
rospy.loginfo("detect marker")
pub.publish(msg)
rate.sleep()
cv2.imshow("img", img)
cv2.waitKey(1)
else:
cv2.imshow("img", frame)
cv2.waitKey(1)
msg = Visual_msg()
msg.id = 100
msg.rvec.x = 0
msg.rvec.y = 0
msg.rvec.z = 0
msg.tvec.x = 0
msg.tvec.y = 0
msg.tvec.z = 0
rospy.loginfo("detect nothing")
rospy.loginfo(msg)
pub.publish(msg)
def main_boi():
while (True):
ret, frame = cap.read()
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.getPredefinedDictionary(0)
parameters = aruco.DetectorParameters_create()
# draw boundary
cv2.rectangle(frame, (50, 50), (1230, 670), (255, 0, 0), 2)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame, aruco_dict, parameters=parameters)
for box_index in range(len(corners)):
x_avg = get_avg_coord(corners[box_index][0])[0]
y_avg = get_avg_coord(corners[box_index][0])[1]
cv2.rectangle(frame, (int(x_avg), int(y_avg)),
(int(x_avg + 5), int(y_avg + 5)), (0, 255, 0), 2)
# if start
if ids[box_index][0] == start_id:
start = corners[box_index][0]
print "start: " + str(get_avg_coord(start))
# if goal
elif ids[box_index][0] == goal_id:
goal = corners[box_index][0]
print "goal: " + str(get_avg_coord(goal))
# if obs
for i in range(len(obs_id)):
if ids[box_index][0] in obs_id:
obs[i] = get_avg_coord(corners[box_index][0])
print "obs: " + str(obs)
# if robot
for i in range(len(robot_id)):
if ids[box_index][0] in robot_id:
robot[i] = get_avg_coord(corners[box_index][0])
print "robot: " + str(robot)
cv2.putText(
frame, "id=" + str(ids[box_index][0]) + " (" +
str(int(x_avg)) + "," + str(int(y_avg)) + ")",
(int(x_avg), int(y_avg)), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255))
gray = aruco.drawDetectedMarkers(frame, corners)
draw_grid(frame, num_rows, num_cols, corners, ids)
# Display the resulting frame
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def detect_aruco(self):
self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
image = self.image_repository.get_next_image()
gray = cv2.cvtColor(image.frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray,
self.aruco_dict,
parameters=aruco.DetectorParameters_create())
return corners
def init(debug=False):
global sock
sock = SocketClient(timeout=1)
global capture
capture = CaptureThread(debug)
capture.start()
global markerDict
markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
def aruco_detect1():
aruco_list = {}
rgb = image1[...,::-1]
gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_1000)#aruco.Dictionary_get(aruco.DICT_4X4_50) #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249
print(aruco_dict)
parameters = aruco.DetectorParameters_create() #refer opencv page for clarification
#lists of ids and the corners beloning to each id
print(parameters)
corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
#corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left.
#print len(corners), corners, ids
print(corners)
print(len(corners))
gray = aruco.drawDetectedMarkers(gray, corners,ids)
cv2.imshow('frame',gray)
print (type(corners[0]))
if len(corners): #returns no of arucos
print (len(corners))
print (len(ids))
for k in range(len(corners)):
temp_1 = corners[k]
temp_1 = temp_1[0]
temp_2 = ids[k]
temp_2 = temp_2[0]
aruco_list[temp_2] = temp_1
return aruco_list
img = cv2.imread(path_to_image) #give the name of the image with the complete path
id_aruco_trace = 0
det_aruco_list = {}
img2 = img[0:x,0:y,:] #separate out the Aruco image from the whole image
det_aruco_list = detect_Aruco(img2)
if det_aruco_list:
img3 = mark_Aruco(img2,det_aruco_list)
id_aruco_trace = calculate_Robot_State(img3,det_aruco_list)
print(id_aruco_trace)
cv2.imshow('image',img2)
cv2.waitKey(0)
cv2.destroyAllWindows()
def show_aruco(self):
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
image = aruco.drawMarker(aruco_dict, 3, 700)
photo = ImageTk.PhotoImage(image=PIL.Image.fromarray(image))
self.projector_window.canvas.create_image(500, 400, image=photo)
return np.array(
[[150, 50, 0], [850, 50, 0], [850, 750, 0], [150, 750, 0]],
dtype=np.float32)
def gen_aruco(n):
while(n):
img = np.zeros((100,100,3), np.uint8)
Dictionary = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
img=aruco.drawMarker(Dictionary,n,100,img,1)
#cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\'+str(n)+'.jpg',img)
cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\class_0\\' + 'aruco_0_'+str(n) + '.jpg', img)
print('gen_aruco: '+'aruco_0_'+str(n) + '.jpg')
n = n - 1
return 0
def get_markers(frame):
'''Função que pega os marcadores da frame passada como argumento e retorna três listas
corners (com as coordenadas de cada ID detectado), ids (com o ID detectado) e rejectedImgPoints
(com as coordenadas rejeitada)'''
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame, aruco_dict, parameters=parameters)
return corners, ids, rejectedImgPoints
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 create_markers():
j = 2
markers_dict_1 = aruco.getPredefinedDictionary(aruco.DICT_6X6_50)
markers_dict1 = custom_dictionary_from(j, 6, markers_dict_1)
markers_dict_2 = aruco.getPredefinedDictionary(aruco.DICT_7X7_50)
markers_dict2 = custom_dictionary_from(j, 7, markers_dict_2)
markers1 = list(
aruco.drawMarker(markers_dict1, i + 1, 200) for i in range(1))
markers2 = list(
aruco.drawMarker(markers_dict2, i + 1, 200) for i in range(1))
for i in range(len(markers1)):
cv2.imwrite(
os.path.join('markers', 'marker6_') + str(i + 1) + '.png',
markers1[i])
for i in range(len(markers2)):
cv2.imwrite(
os.path.join('markers', 'marker7_') + str(i + 1) + '.png',
markers2[i])
return markers_dict1, markers_dict2
def __threadfunc__(self):
markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
while self.working:
ok, frame = self.capture.read()
self.current_frame = frame
if self.debug:
(corners, ids, rejects) = aruco.detectMarkers(frame, markerDict)
frame = aruco.drawDetectedMarkers(frame, corners, ids)
cv2.imshow("Camera", cv2.flip(imutils.resize(frame, width=480), 1))
cv2.waitKey(1)
