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 getAndSearchImage(self): global drone global thread_lock_camera_frame global render_frame global new_frame global W global H try: # print pygame.image pixelarray = drone.get_image() if pixelarray != None: frame = pixelarray[:,:,::-1].copy() #resize image resized=cv2.resize(frame,(W,H)) # aruco detection gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() #lists of ids and the corners beloning to each id corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) # iterate over all found markers to determine the one to follow (biggest one) selected_corners = [] max_index = (0,0) counter = 0 if len(corners) > 0: dim = corners[0].shape #print dim while counter<dim[0]: tmp_corners = ((corners[0])[counter]) # A=(1/2)|[(x3-x1)(y4-y2) +(x4-x2)(y1-y3)]| area = 0.5*((tmp_corners[2][0] - tmp_corners[0][0]) * (tmp_corners[3][1] - tmp_corners[1][1]) + (tmp_corners[3][0] - tmp_corners[1][0]) * (tmp_corners[0][1] - tmp_corners[2][1])) if area > max_index[0]: max_index = (area,counter) counter +=1 max_corners = ((corners[0])[max_index[1]]) selected_corners = np.array([np.array([(corners[0])[max_index[1]]],dtype=np.float32)])#[max_index[0]*4:max_index[0]*4+3] # draw all markers display = aruco.drawDetectedMarkers(resized, corners) thread_lock_camera_frame.acquire() render_frame = display new_frame = True thread_lock_camera_frame.release() # prepare function output if len(selected_corners) > 0: x,y = max_corners.sum(axis=0)/4 area = max_index[0] else: x = -W y = -1 area = -1 return (x-W/2,y-H/2), area except: pass
def render(self):
# Start Output Image
# Create a blank image
if self.ui.displayAll():
widthAll = 0
heightAll = 0
for z in self.zones:
widthAll += z.width
heightAll = z.height
outputImg = zeros((heightAll, widthAll, 3), uint8)
elif size(self.zones) > 0:
outputImg = zeros((self.zones[self.ui.display].height, self.zones[self.ui.display].width, 3), uint8)
else:
outputImg = zeros((1080, 1920, 3), uint8)
for z in self.zones:
if z.calibrated:
#z.projector.outputZoneImage()
z.projector.outputCalibrationImage()
else:
z.projector.outputCalibrationImage()
if self.ui.isDisplayed(z.id):
# Prepare image based on display mode.
if self.ui.displayMode == self.ui.DATAONLY:
img = zeros((z.height, z.width, 3), uint8) # Create a blank image
else:
img = z.image
# If we are only displaying the source image we are done.
if self.ui.displayMode == self.ui.SOURCE:
if self.ui.displayAll():
outputImg[0:z.height, z.id*z.width:(z.id+1)*z.width] = img
else:
outputImg = img
continue;
# Draw Objects on Scanner window
# Crosshair in center
pt0 = (z.width/2, z.height/2-5)
pt1 = (z.width/2, z.height/2+5)
cv2.line(img, pt0, pt1, self.ui.COLOR_PURPLE, 1)
pt0 = (z.width/2-5, z.height/2)
pt1 = (z.width/2+5, z.height/2)
cv2.line(img, pt0, pt1, self.ui.COLOR_PURPLE, 1)
# Draw the markers
if not z.calibrated:
outputImg = aruco.drawDetectedMarkers(z.image, self.corners)
if self.ui.displayAll():
outputImg[0:z.height, z.id*z.width:(z.id+1)*z.width] = img
else:
outputImg = img
return outputImg
def get_aruco_posture(self, id):
ret, frame = self.cap.read()
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
arucoParams = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame, aruco_dict, parameters=arucoParams)
id_is_detected = False
transform_matrix = None
if ids is not None:
for i in ids:
index_in_list = list(ids).index(i)
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners[index_in_list], 0.05, self.cameraMatrix,
self.distCoeffs)
#print(tvec)
aruco.drawAxis(frame, self.cameraMatrix, self.distCoeffs,
rvec[0, :, :], tvec[0, :, :], 0.03)
aruco.drawDetectedMarkers(frame, [corners[index_in_list]])
corners_array = corners[index_in_list][0]
_, max_y_index = np.argmax(corners_array, axis=0)
lowest_corner = corners_array[max_y_index, :]
position4Text = np.copy(lowest_corner)
position4Text[1] += 20
# draw conners and axis onto frame
if i == marker_id.ROBOT1:
cv2.putText(frame, "robot1(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
#print("Robot marker detected.")
elif i == marker_id.ROBOT2:
cv2.putText(frame, "robot2(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
elif i == marker_id.ROBOT3:
cv2.putText(frame, "robot3(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
elif i == marker_id.GOAL:
cv2.putText(frame, "goal(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
#print("Goal marker detected.")
elif i == marker_id.ORIGIN:
cv2.putText(frame, "origin(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
#print("Origin marker detected.")
elif i == marker_id.OBSTACLE:
cv2.putText(frame, "obstacle(Id: " + str(i) + ")",
tuple(position4Text), self.font, 0.7,
(0, 255, 0), 2, cv2.LINE_AA)
#print("Obstacle marker detected.")
# output the transform that we want
if i == id:
id_is_detected = True
transform_matrix = np.zeros([4, 4])
transform_matrix[0:3, 3] = tvec[0, 0, :]
rotationMatrix, _ = cv2.Rodrigues(rvec[0, 0, :])
transform_matrix[0:3, 0:3] = rotationMatrix
transform_matrix[3, 3] = 1
#print(" And we need to output its transformation matrix with respect to camera:")
#print(" ", transform_matrix)
# if the transform that we want is not detected, output some debug information and tranform matrix should be none and result is false
if not id_is_detected:
if id == 1:
print("Robot1 marker not detected!")
if id == 2:
print("Robot2 marker not detected!")
if id == 3:
print("Robot3 marker not detected!")
elif id == 4:
print("Goal marker not detected! ")
elif id == 5:
print("Origin marker not detected!")
elif id == 6:
print("Obstacle marker not detected!")
else:
print("No marker detected!")
# display the frame
cv2.imshow("frame", frame)
return id_is_detected, transform_matrix
markerLength = 15.8 #Dimensions can be anything, but will match what is later returned in tvec
markerSeparation = .7 #only matters for boards
board = aruco.GridBoard_create(5, 7, markerLength, markerSeparation, aruco_dict)
arucoParams = aruco.DetectorParameters_create() #Just use this
for i in range(1,10000): #Can also be a while True loop
cap.set(cv2.CAP_PROP_AUTOFOCUS, 0) #Stop camera from refocusing, if possible?
cap.set(cv2.CAP_PROP_FOCUS, 0)
ret, imgRemapped = cap.read() #returns an image. stores it in imgRemapped
## print(i)
imgRemapped_gray = cv2.cvtColor(imgRemapped, cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image
corners, ids, rejectedImgPoints = aruco.detectMarkers(imgRemapped_gray, aruco_dict, parameters=arucoParams) # Detect aruco marker.
if ids != None: # if aruco marker detected
rvec, tvec = aruco.estimatePoseSingleMarkers(corners, markerLength, camera_matrix, dist_coeffs) # For a single marker
imgWithAruco = aruco.drawDetectedMarkers(imgRemapped, corners, ids, (0,255,0)) #Draws a box around the marker based on its corners, in green. Labels it with its id.
imgWithAruco = aruco.drawAxis(imgWithAruco, camera_matrix, dist_coeffs, rvec, tvec, 12) # axis length is last parameter. The unit of this matches input unit. Can be feet, inches, anything.
time.sleep(.001)
x,y,z = tvec[0][0][0], tvec[0][0][1], tvec[0][0][2] #tvec stores position
print(x,y,z, ids[0][0])
x = str(x)
y = str(y)
z = str(z)
ser.write(z)
#ser.write(' ')
if(ids[0][0] == 1): #If I see the first marker, draw red
color = 'r'
if(ids[0][0] == 2): #If I see second marker, draw blue
color = 'b'
else:
color = 'g'
isColor=True)
while True:
try:
ret, img = cam.read()
corners, ids, reprojImgPts = aruco.detectMarkers(img, ar_dict)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, 15.0, camera_matrix, dist_coeffs)
# print('map_rvecs', map_rvecs)
dist_cm.append(np.linalg.norm(tvecs[1] - tvecs[0]))
# print(dist_cm[-1])
timestamps.append(time.time() - starttime)
img = aruco.drawDetectedMarkers(img, corners, ids)
img = aruco.drawAxis(img, camera_matrix, dist_coeffs, rvecs[0],
tvecs[0], 5.0)
img = aruco.drawAxis(img, camera_matrix, dist_coeffs, rvecs[1],
tvecs[1], 5.0)
cv2.line(img, tuple(corners[0][0][0]), tuple(corners[1][0][0]),
(0, 0, 255), 1, cv2.LINE_AA)
smoothed_cm.append(smooth(dist_cm[-5:], 5))
points.setData(timestamps, smoothed_cm, pen='r')
cv2.putText(img, "team " + args.id, (550, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
if len(smoothed_cm) > 100:
mins = argrelmin(np.array(smoothed_cm),
tlx = 0
tly = 0
blx = 0
bly = 0
trix = 0
triy = 0
brx = 0
bry = 0
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
gray = clahe.apply(gray)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
gray = aruco.drawDetectedMarkers(gray, corners, ids)
cv2.imshow('frame', gray)
for a in corners:
tlx = a[0][0][0]
tly = a[0][0][1]
trix = a[0][1][0]
triy = a[0][1][1]
blx = a[0][3][0]
bly = a[0][3][1]
brx = a[0][2][0]
bry = a[0][2][1]
if ret == True and (tlx, tly, trix, triy, blx, bly, brx,
bry) == (0, 0, 0, 0, 0, 0, 0, 0):
out.write(frame)
markerLength = 350
camera_matrix = np.asmatrix([[613.299988, 0.0, 354.949005],
[0.0, 0.0, 612.106018], [214.380005, 0.0, 1.0]])
dist_coeffs = (-0.439, 0.263, 0.001, 0.0, -0.113)
while (True):
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) #chib change to 4x4)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if type(ids) != type(None):
rvec, tvec, z = aruco.estimatePoseSingleMarkers(
corners, markerLength, camera_matrix, dist_coeffs)
#rvec, tvec = aruco.estimatePoseSingleMarkers(corners, markerLength, camera_matrix, dist_coeffs)
imgWithAruco = aruco.drawDetectedMarkers(gray, corners, ids)
imgWithAruco = aruco.drawAxis(imgWithAruco, camera_matrix, dist_coeffs,
rvec, tvec, 100)
cv2.imshow('frame', imgWithAruco)
else:
pass
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
[0.11330149, -0.27730294, -0.00748054, 0.00234142, 0.28207847])
# generate aruco markers and save them in the local dir
for i in range(5):
a = aruco.drawMarker(aruco_dict, i, sidePixels=200, borderBits=1)
cv2.imwrite('aruco{}.jpg'.format(i), a)
cv2.namedWindow("preview") # created a window named preview
vc = cv2.VideoCapture(0) # set up the video capture from the webcam
while True:
rval, frame = vc.read() # read a new frame from the webcam.
if rval: # if we got a new frame
corners, ids, regjected = aruco.detectMarkers(
frame, aruco_dict) # run aruco on the frame
detected_frame = aruco.drawDetectedMarkers(
frame, corners, ids) # draw the boxes on the detected markers
rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners,
markerEdgeLength,
cameraMatrix,
distCoeffs)
if rvecs != None:
rot = cv2.Rodrigues(rvecs)[0] # get the rotation matrix
inv_rot = np.transpose(rot) # invert it
new_translation = np.matmul(inv_rot, np.multiply(
tvecs[0][0], -1.0)) # apply it to the translation matrix
print new_translation
axis_frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvecs,
tvecs, 7.39)
cv2.imshow("preview", axis_frame) # display the frame
euler_angle[2] -= 360
if euler_angle_cube[2] < -180:
euler_angle_cube[2] += 360
elif euler_angle_cube[2] > 180:
euler_angle_cube[2] -= 360
# display annotations (IDs and pose)
cv.putText(imageCopy, "Cozmo Pose", (10, 20),
cv.FONT_HERSHEY_PLAIN, 1, (255, 0, 0, 0))
msg = "X(m): " + str(tvecs[index][0][0])
cv.putText(imageCopy, msg, (10, 45), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
msg = "Y(m): " + str(tvecs[index][0][1])
cv.putText(imageCopy, msg, (10, 70), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
msg = "Angle(deg): " + str(euler_angle[2])
cv.putText(imageCopy, msg, (10, 95), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
msg = "Cube X(m): " + str(tvecs[index2][0][0])
cv.putText(imageCopy, msg, (10, 120), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
msg = "Cube Y(m): " + str(tvecs[index2][0][1])
cv.putText(imageCopy, msg, (10, 145), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
msg = "Cube Angle(deg): " + str(euler_angle_cube[2])
cv.putText(imageCopy, msg, (10, 170), cv.FONT_HERSHEY_PLAIN, 1,
(255, 0, 0, 0))
aruco.drawDetectedMarkers(imageCopy, markerCorners, markerIds)
cv.imshow("Detect Markers", imageCopy)
cv.waitKey(100)
target_found = 1
robot_position = robot_center[0] #get x position
turn_percent = int((robot_position - goal_position) / width * 254 +
127)
else:
target_found = 0
turn_percent = 0
target_distance = 0
if robot_distance is not None:
target_distance = int(robot_distance)
else:
target_distance = int(0)
#save to video
if (args.output_name is not None):
frame_trim = aruco.drawDetectedMarkers(frame_trim, corners)
print(frame_trim.shape, trim_height)
cap.write(frame_trim, add_time=time_)
#show print outs
if (args.verbose):
if (turn_percent == 0): turn_str = "Not Found."
elif (turn_percent < 128): turn_str = "Turning right."
else: turn_str = "Turning left."
print("Position:", turn_percent, "Target Distance: ", target_distance,
turn_str)
message.write_array([turn_percent, target_distance])
processing_number += 1
'''
#lists of ids and the corners beloning to each id
center = (0, 0)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
if corners:
for corner in corners[0]:
dim = corner.shape
print corner
s = np.sum(corner, axis=0)
print "sum", s
s = s / dim[0]
center = (s[0], s[1])
#It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
gray = aruco.drawDetectedMarkers(gray, corners)
color = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
cv2.circle(color, center, 2, (0, 0, 255), 2)
#print(rejectedImgPoints)
# Display the resulting frame
cv2.imshow('frame', color)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
fig = plt.figure()
while True:
ret, frame = cap.read()
# gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray = frame
res = aruco.detectMarkers(gray, dictionary)
#res = contains array of corners of detected markers, ID of markers, rejected points
i = 0
if len(res[0]) > 0:
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
res[0], length, mtx, dist)
# print tvec.shape
# print "aa gaya"
aruco.drawDetectedMarkers(gray, res[0], res[1])
x = 0
for x in xrange(0, len(rvec)):
# print ("x = " ,x)
a = res[1]
print a
# print type(rvec)
print rvec.shape
if rvec.shape == (10, 1, 3):
# print random.random()
print rvec.shape
if a.item(x) == 9: #detects marker number 7
aruco.drawAxis(gray, mtx, dist, rvec[x], tvec[x], length)
# cv2.Rodrigues(rvec, R)
print "aa gaya"
async def startSending():
async with websockets.connect('ws://python3js.glitch.me/') as ws:
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
# Creating a theoretical board we'll use to calculate marker positions
board = aruco.GridBoard_create(
markersX=5,
markersY=7,
markerLength=0.04,
markerSeparation=0.01,
dictionary=aruco_dict)
mtx = None
dist = None
with open('callibrationData.pickle', 'rb') as handle:
callibrationData = pickle.load(handle)
dist = callibrationData["dist"]
mtx = callibrationData["mtx"]
# while(cam.isOpened()):
# # Capturing each frame of our video stream
# ret, QueryImg = cam.read()
# if ret == True:
# # grayscale image
# gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# parameters = aruco.DetectorParameters_create()
# # Detect Aruco markers
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# # Make sure markers were detected before continuing
# if ids is not None and corners is not None and len(ids) > 0 and len(corners) > 0 and len(corners) == len(ids):
# # The next if makes sure we see all matrixes in our gridboard
# if len(ids) == len(board.ids):
# # Calibrate the camera now using cv2 method
# ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
# objectPoints=board.objPoints,
# imagePoints=corners,
# imageSize=gray.shape, #[::-1], # may instead want to use gray.size
# cameraMatrix=None,
# distCoeffs=None)
# # # Calibrate camera now using Aruco method
# # ret, cameraMatrix, distCoeffs, _, _ = aruco.calibrateCameraAruco(
# # corners=corners,
# # ids=ids,
# # counter=35,
# # board=board,
# # imageSize=gray.shape[::-1],
# # cameraMatrix=None,
# # distCoeffs=None)
# # Print matrix and distortion coefficient to the console
# break
# print("Callibrated!")
# print(dist)
# print("")
# print(mtx)
# callibrationData = {"dist": dist, "mtx": mtx}
#with open('callibrationData.pickle', 'wb') as handle:
#pickle.dump(callibrationData, handle, protocol=pickle.HIGHEST_PROTOCOL)
cap = cv2.VideoCapture(4)
time.sleep(2)
while (True):
ret, frame = cap.read()
# 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_1000)
# 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)
#(rvec-tvec).any() # get rid of that nasty numpy value array error
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1)
xPos = tvec[0][0][0]
yPos = tvec[0][0][1]
zPos = tvec[0][0][2]
rmat = cv2.Rodrigues(rvec[i])[0]
yawpitchroll_radian = -rotationMatrixToEulerAngles(rmat)
yawpitchroll_radian[0] = yawpitchroll_radian[0]-math.pi
yawpitchroll_angles = 180*yawpitchroll_radian/math.pi
zRotation = yawpitchroll_angles[2]
yRotation = yawpitchroll_angles[1]
xRotation = yawpitchroll_angles[0]
if(xRotation < 0):
xRotation = 360 - abs(xRotation)
if(yRotation < 0):
yRotation = 360 - abs(yRotation)
if(zRotation < 0):
zRotation = 360 - abs(zRotation)
#print("x: " + str(xRotation))
print(zRotation)
# Checks if a matrix is a valid rotation matrix.
await ws.send(str(xPos) + "|" + str(yPos) + "|" + str(zPos) + "|" + str(xRotation) + "|" + str(yRotation) + "|" + str(zRotation))
# 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)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
parameters = aruco.DetectorParameters_create()
#parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
cornersA, idsA, rejectedImgPointsA = aruco.detectMarkers(
gray1, aruco_dict, parameters=parameters)
cornersB, idsB, rejectedImgPointsB = aruco.detectMarkers(
gray2, aruco_dict, parameters=parameters)
if ((idsA is not None) & (idsB is not None)):
if ((len(idsA) == 1) & (len(idsB) == 1)):
objpoints3.append(objp2)
imgpoints3.append(cornersA)
objpoints4.append(objp2)
imgpoints4.append(cornersB)
aruco.drawDetectedMarkers(frame1, cornersA)
# # 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(idsA != None) & np.all(idsB != None):
# print('in')
# estimate pose of each marker and return the values
# rvet and tvec-different from camera coefficients
rvec1, tvec1, _ = aruco.estimatePoseSingleMarkers(
cornersA, 0.05, mtx1, dist1)
rvec2, tvec2, _ = aruco.estimatePoseSingleMarkers(
cornersB, 0.05, mtx2, dist2)
r1, _ = cv2.Rodrigues(rvec1)
r2, _ = cv2.Rodrigues(rvec2)
def main(msg):
global cmd_vel_pub, distance, upword, downward, pub, take_off
global height
# --- Get the camera calibration path
twist = Twist()
calib_path = ""
camera_matrix = np.loadtxt(
"/home/vishal/vishal_testing/drone-simulation-ws/src/drone_control/src/cameraMatrix.txt",
delimiter=',')
camera_distortion = np.loadtxt(
'/home/vishal/vishal_testing/drone-simulation-ws/src/drone_control/src'
'/cameraDistortion.txt',
delimiter=',')
# --- 180 deg rotation matrix around the x axis
R_flip = np.zeros((3, 3), dtype=np.float32)
R_flip[0, 0] = 1.0
R_flip[1, 1] = -1.0
R_flip[2, 2] = -1.0
# --- Define the aruco dictionary
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()
# --- Capture the videocamera (this may also be a video or a picture)
cap = cv_bridge.CvBridge()
# -- Font for the text in the image
font = cv2.FONT_HERSHEY_PLAIN
# while True:
# -- Read the camera frame
frame = cap.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# cap = cv2.VideoCapture(frame)
# ret, frame = cap.read(frame)
# -- 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=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
request = "True"
if ids is None or take_off == "true":
twist.linear.z = 0.5
cmd_vel_pub.publish(twist)
if ids is not None and ids[0] == id_to_find:
take_off == "false"
pub.publish(request)
print " Landing Pad Found"
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix, 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)
# marker = cap.imgmsg_to_cv2(msg,desired_encoding='bgr8')
marks = cap.imgmsg_to_cv2(msg, desired_encoding='bgr8')
marker = cv2.rectangle(marks,
(corners[0][0][1][0], corners[0][0][1][1]),
(corners[0][0][3][0], corners[0][0][3][1]),
(45, 255, 90), -1)
hsv = cv2.cvtColor(marker, cv2.COLOR_BGR2HSV)
LowerYellow = numpy.array([29, 86, 6])
UpperYellow = numpy.array([64, 255, 255])
mask = cv2.inRange(hsv, LowerYellow, UpperYellow)
# Calculating the the mask coordinates on marker
h, w, d = marker.shape
Top_Search = 3
bottom_Search = Top_Search + 300
mask[0:Top_Search, 0:w] = 0
mask[bottom_Search:h, 0:w] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
dx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
cv2.circle(marker, (dx, cy), 1, (0, 0, 255), -1)
# Calculating the error value that how much motors has to rotate
err_x = dx - w / 2
err_y = cy - h / 2
twist.linear.x = -float(err_y) / 95
twist.angular.z = -float(err_x) / 95
twist.linear.z = -0.2
if height <= 0.5:
downward = 1
cmd_vel_pub.publish(twist)
if downward == 1:
twist.angular.z = 0
twist.linear.x = 0
twist.linear.z = -0.5
print "landing"
cmd_vel_pub.publish(twist)
# --- Display the frame
cv2.imshow('bottom_camera', frame)
# --- use 'q' to quit
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
def custom_detect_Aruco(
img, n,
C): # returns the detected aruco list dictionary with id: corners
aruco_list = {}
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if n == 'ARUCO' and C == 'ORIGINAL':
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
elif n == '4':
if C == '50':
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
elif C == '100':
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100)
elif C == '250':
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
elif C == '1000':
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
elif n == '5':
if C == '50':
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50)
elif C == '100':
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_100)
elif C == '250':
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
elif C == '1000':
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
elif n == '6':
if C == '50':
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
elif C == '100':
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_100)
elif C == '250':
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
elif C == '1000':
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
elif n == '7':
if C == '50':
aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_50)
elif C == '100':
aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_100)
elif C == '250':
aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_250)
elif C == '1000':
aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_1000)
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
scale_percent = 50 # percent of original size
width = int(frame.shape[1] * scale_percent / 100)
height = int(frame.shape[0] * scale_percent / 100)
dim = (width, height)
# resize image
frame = cv2.resize(frame, dim, interpolation=cv2.INTER_AREA)
# Check if frame is not empty
if not ret:
continue
# Set AR Marker
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
corners, ids, _ = aruco.detectMarkers(frame,
aruco_dict,
parameters=parameters)
frame = aruco.drawDetectedMarkers(frame, corners, ids)
# Display the resulting frame
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Release everything:
cap.release()
cv2.destroyAllWindows()
# cv2.imwrite(filename, img)
imgRemapped = cv2.remap(img, map1, map2, cv2.INTER_LINEAR,
cv2.BORDER_CONSTANT) # for fisheye remapping
imgRemapped_gray = cv2.cvtColor(
imgRemapped,
cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image
filename = "remappedgray" + str(i).zfill(3) + ".jpg"
cv2.imwrite(filename, imgRemapped_gray)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
imgRemapped_gray, aruco_dict, parameters=arucoParams) # Detect aruco
if ids != None: # if aruco marker detected
rvec, tvec, trash = aruco.estimatePoseSingleMarkers(
corners, markerLength, camera_matrix,
dist_coeffs) # posture estimation from a single marker
imgWithAruco = aruco.drawDetectedMarkers(imgRemapped, corners, ids,
(0, 255, 0))
imgWithAruco = aruco.drawAxis(
imgWithAruco, camera_matrix, dist_coeffs, rvec, tvec, 100
) # axis length 100 can be changed according to your requirement
filename = "pose" + str(count).zfill(3) + "_" + str(i).zfill(
3) + ".jpg"
cv2.imwrite(filename, imgWithAruco)
count += 1
else: # if aruco marker is NOT detected
imgWithAruco = imgRemapped # assign imRemapped_color to imgWithAruco directly
cv2.imshow("aruco", imgWithAruco) # display
if cv2.waitKey(10) & 0xFF == ord('q'): # if 'q' is pressed, quit.
break
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global resultados
global c_amarelo
global distance
global id_to_find
global achou
global state
global maior_contorno
global achou_creeper
global estado_anterior
global state_cor
global ver_cor
missao = ['green']
#missao=['red']
#missao=['blue']
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
# print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay == True:
# Esta logica do delay so' precisa ser usada com robo real e rede wifi
# serve para descartar imagens antigas
print("Descartando por causa do delay do frame:", delay)
return
try:
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, saida_net, resultados = visao_module.processa(temp_image)
segmentado_cor = verifica_cor(temp_image, missao[0])
contornos, arvore = cv2.findContours(segmentado_cor.copy(),
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
maior_contorno_area = 0
for cnt in contornos:
area = cv2.contourArea(cnt)
if area > maior_contorno_area:
maior_contorno = cnt
maior_contorno_area = area
if not maior_contorno is None:
cv2.drawContours(saida_net, [maior_contorno], -1, [0, 0, 255], 5)
maior_contorno = np.reshape(maior_contorno,
(maior_contorno.shape[0], 2))
media = maior_contorno.mean(axis=0)
media = media.astype(np.int32)
cv2.circle(saida_net, (media[0], media[1]), 5, [0, 255, 0])
if ver_cor:
print(cv2.contourArea(maior_contorno))
if cv2.contourArea(maior_contorno) >= 1000:
achou_creeper = True
estado_anterior = state
state = 'cor'
ver_cor = False
state_cor = 0
gray = cv2.cvtColor(temp_image, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if ids is not None:
aruco.drawDetectedMarkers(saida_net, corners, ids)
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix,
camera_distortion)
rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :]
if ids[0] == id_to_find:
achou = True
distance = np.sqrt(tvec[0]**2 + tvec[1]**2 + tvec[2]**2)
mask_amarelo = filter_color(temp_image, low_yellow, high_yellow)
#if mask_amarelo.Contours is not None:
c_amarelo = center_of_mass(mask_amarelo)
crosshair(saida_net, c_amarelo, 3, (255, 0, 0))
# Desnecessário - Hough e MobileNet já abrem janelas
cv_image = saida_net.copy()
#cv2.imshow("cv_image", cv_image)
cv2.imshow("cv_image", cv_image)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def detectMarker():
id_to_find = 24
marker_size = 10 #- [cm]
#--- Get the camera calibration path
calib_path = ""
camera_matrix = np.loadtxt(calib_path + 'cameraMatrix_raspi.txt',
delimiter=',')
camera_distortion = np.loadtxt(calib_path +
'cameraDistortion_raspi.txt',
delimiter=',')
#--- Define the aruco dictionary
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()
#--- Capture the videocamera (this may also be a video or a picture)
cap = cv2.VideoCapture(0)
#-- Set the camera size as the one it was calibrated with
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
#-- Font for the text in the image
font = cv2.FONT_HERSHEY_PLAIN
while True:
#-- Read the camera frame
ret, frame = cap.read()
#-- 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=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
if ids is not None and ids[0] == id_to_find:
#-- ret = [rvec, tvec, ?]
#-- array of rotation and position of each marker in camera frame
#-- rvec = [[rvec_1], [rvec_2], ...] attitude of the marker respect to camera frame
#-- tvec = [[tvec_1], [tvec_2], ...] position of the marker in camera frame
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix,
camera_distortion)
#-- Unpack the output, get only the first
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
print("found / detected marker ! .. . . .!")
#--- Display the frame
cv2.imshow('frame', frame)
#--- use 'q' to quit
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cap.release()
cv2.destroyAllWindows()
break
# (640, 480), 1, (640, 480))
# dst = cv.undistort(frame, mtx, dist, None, new_mtx)
gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
aruco_dict,
parameters=params)
if ids is not None:
print("we found: ")
for n in range(len(ids)):
print(str(ids[n]) + " at " + str(corners[n][0][0]) +
" with distance ")
# here I can calculate distances and populate a matrix
# here I can tx via radio
# this is only for debugging
aruco.drawDetectedMarkers(frame, corners, ids, (0, 0, 255))
for i in corners:
pos = (int(sum([j[0] for j in i[0]])/4),
int(sum([k[1] for k in i[0]])//4))
txt_pos = pos[0], pos[1] - 20
cv.circle(frame, pos, 1, (0, 0, 255), -1) # BGR color (red)
cv.putText(frame, '{}'.format(pos),
txt_pos, font, 0.3, (0, 255, 0)) # BGR color (green)
#for n in range(len(ids)):
# print(str(ids[n])+" at "+str(pos[0])+" ")
# rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners,
# 0.05, mtx, dist)
# for i in range(len(ids)):
# aruco.drawAxis(dst, mtx, dist, rvecs[i], tvecs[i], 0.05)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
global ret, mtx, dist, rvecs, tvecs
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if np.all(ids != None):
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners[0], 0.05, mtx, dist)
topleftX = corners[0][0][0][0]
topleftY = corners[0][0][0][1]
toprightX = corners[0][0][1][0]
toprightY = corners[0][0][1][1]
bottomleftX = corners[0][0][2][0]
bottomlextY = corners[0][0][2][1]
bottomrightX = corners[0][0][3][0]
bottomrightY = corners[0][0][3][1]
# expected information
distance = tvec[0][0][2]
midpointX = (topleftX + bottomrightX) / 2
midpointY = (topleftY + bottomrightY) / 2
print("topleft corner x {}".format(topleftX))
print("topleft corner y {}".format(topleftY))
print("topright corner x {}".format(toprightX))
print("topright corner y {}".format(toprightY))
print("bottomleft corner x {}".format(bottomleftX))
print("bottomleft corner y {}".format(bottomlextY))
print("bottomright corner x {}".format(bottomrightX))
print("bottomright corner y {}".format(bottomrightY))
print("distance {}".format(distance))
print("topleft corner x {}".format(topleftX))
print("topleft corner y {}".format(topleftY))
print("topright corner x {}".format(toprightX))
print("topright corner y {}".format(toprightY))
print("bottomleft corner x {}".format(bottomleftX))
print("bottomleft corner y {}".format(bottomlextY))
print("bottomright corner x {}".format(bottomrightX))
print("bottomright corner y {}".format(bottomrightY))
print("distance {}".format(distance))
aruco.drawAxis(cv_image, mtx, dist, rvec[0], tvec[0], 0.1)
aruco.drawDetectedMarkers(cv_image, corners)
# draw ID
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(cv_image, "Id: " + str(ids), (0, 64), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
dis = Float32MultiArray()
# dis.layout=(3,1)
dis.data = (distance, midpointX, midpointY)
self.distance_pub.publish(dis)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# SUB PIXEL DETECTION
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.0001)
for corner in corners:
cv2.cornerSubPix(gray,
corner,
winSize=(11, 11),
zeroZone=(-1, -1),
criteria=criteria)
frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
size_of_marker = 0.13 # side lenght of the marker in meter
rvecs, tvecs, obj = aruco.estimatePoseSingleMarkers(
corners, size_of_marker, mtx, dist)
#rvecs,tvecs = aruco.estimatePoseSingleMarkers( mtx, dist)
length_of_axis = 0.01
imaxis = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
key = drone.getKey()
if key == " ":
drone.land()
try:
tvecs = tvecs * 10
xyz = tvecs[0]
data = pd.DataFrame(data=tvecs.reshape(len(tvecs), 3),
columns=["tx", "ty", "tz"],
# gray1 = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
# gray2 = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
dd1 = cv2.undistort(frame1, cm1, dc1)
dd2 = cv2.undistort(frame2, cm2, dc2)
corners1, ids1, rejectedImgPoints1 = aruco.detectMarkers(frame1, aruco_dict, parameters=parameters)
corners2, ids2, rejectedImgPoints2 = aruco.detectMarkers(frame2, aruco_dict, parameters=parameters)
ud1, ids11, rejectedImgPoints11 = aruco.detectMarkers(dd1, aruco_dict, parameters=parameters)
ud2, ids22, rejectedImgPoints22 = aruco.detectMarkers(dd2, aruco_dict, parameters=parameters)
gray1 = aruco.drawDetectedMarkers(frame1, corners1)
gray2 = aruco.drawDetectedMarkers(frame2, corners2)
# print(corners2[0])
# print(ud2[0])
# exit()
try:
if g==0:
g=g+1
points4D = cv2.triangulatePoints(P1, P2, ud1[0], ud2[0])
# print(points4D)
distCoeff=camera_distortion)
if ids is not None and ids[0] == id_to_find:
#-- ret = [rvec, tvec, ?]
#-- array of rotation and position of each marker in camera frame
#-- rvec = [[rvec_1], [rvec_2], ...] attitude of the marker respect to camera frame
#-- tvec = [[tvec_1], [tvec_2], ...] position of the marker in camera frame
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix, 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)
#-- Print the tag position in camera frame
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
#-- Get the attitude in terms of euler 321 (Needs to be flipped first)
roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(
R_flip * R_tc)
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image = gray,
board = board,
detectedCorners = corners,
detectedIds = ids,
rejectedCorners = rejectedImgPoints,
cameraMatrix = cameraMatrix,
distCoeffs = distCoeffs)
###########################################################################
# TODO: Add validation here to reject IDs/corners not part of a gridboard #
###########################################################################
# Outline all of the markers detected in our image
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255))
# Require 15 markers before drawing axis
if ids is not None and len(ids) > 15:
# Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video
pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs)
if pose:
# Draw the camera posture calculated from the gridboard
QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 0.3)
# Display our image
cv2.imshow('QueryImage', QueryImg)
# Exit at the end of the video on the 'q' keypress
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Our operations on the frame come here
gray = frame #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)
if corners:
aruco.drawDetectedMarkers(gray, corners, borderColor=(0, 240, 123))
# It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
#gray = aruco.drawDetectedMarkers(gray, corners)
# print(rejectedImgPoints)
# Display the resulting frame
cv2.imshow('frame', gray)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
def estimate_pose(self, frame, marker_side, camera_instrinsic, dist):
try:
tracked_pose_msg = TrackedPose()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
#parameters.adaptiveThreshConstant = 10
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
output = [''] * ids.shape[0]
for i, id_ in enumerate(ids):
# estimate pose of each marker and return the values
# rvet and tvec-different from camera coefficients
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners[i], marker_side, camera_instrinsic, dist)
output[id_[0]] = ([rvec, tvec, corners[i]])
frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners,
ids)
# centers = []
#
# for i in range(len(ids)):
# c = corners[i][0]
# center = (int(c[:, 0].mean()), int(c[:, 1].mean()))
# centers.append(center)
#
## self.assign_markers(ids)
#
# texts = ["base","ef","work"]
# for i,j in enumerate([self.robot_base_id,self.robot_ef_id, self.workpiece_id]):
#
# text = texts[i] + str(j)
# cv2.putText(frame_markers,text,centers[j],cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,0),4,bottomLeftOrigin=False)
# tracked_pose_msg.robot_base_rvec = output[self.robot_base_id][0].squeeze()
# tracked_pose_msg.robot_base_tvec = output[self.robot_base_id][1].squeeze()
tracked_pose_msg.robot_ef_rvec = output[
self.robot_ef_id][0].squeeze()
tracked_pose_msg.robot_ef_tvec = output[
self.robot_ef_id][1].squeeze()
tracked_pose_msg.workpiece_rvec = output[
self.workpiece_id][0].squeeze()
tracked_pose_msg.workpiece_tvec = output[
self.workpiece_id][1].squeeze()
tracked_pose_msg.workpiece_corners_x = np.sort(
(output[self.workpiece_id][2].squeeze())[:, 0])
tracked_pose_msg.workpiece_corners_y = np.sort(
(output[self.workpiece_id][2].squeeze())[:, 1])
tracked_pose_msg.robot_ef_corners_x = np.sort(
(output[self.robot_ef_id][2].squeeze())[:, 0])
tracked_pose_msg.robot_ef_corners_y = np.sort(
(output[self.robot_ef_id][2].squeeze())[:, 1])
# if self.prev_tracked_pose is not None:
# diff_wp_x = np.max(np.abs( tracked_pose_msg.workpiece_corners_x - self.prev_tracked_pose.workpiece_corners_x))
# diff_wp_y = np.max(np.abs(tracked_pose_msg.workpiece_corners_y - self.prev_tracked_pose.workpiece_corners_y))
# diff_r_ef_x = np.max(np.abs(tracked_pose_msg.robot_ef_corners_x - self.prev_tracked_pose.robot_ef_corners_x))
# diff_r_ef_y = np.max(np.abs(tracked_pose_msg.robot_ef_corners_y - self.prev_tracked_pose.robot_ef_corners_y))
#
## print(max(diff_wp_x,diff_wp_y,diff_r_ef_x,diff_r_ef_y))
# if max(diff_wp_x,diff_wp_y,diff_r_ef_x,diff_r_ef_y) > 200:
## rospy.loginfo(tracked_pose_msg)
## rospy.loginfo(self.prev_tracked_pose)
# return
#
self.prev_tracked_pose = tracked_pose_msg
tracked_pose_msg.workpiece_corners_x = tracked_pose_msg.workpiece_corners_x.tolist(
)
tracked_pose_msg.workpiece_corners_y = tracked_pose_msg.workpiece_corners_y.tolist(
)
tracked_pose_msg.robot_ef_corners_x = tracked_pose_msg.robot_ef_corners_x.tolist(
)
tracked_pose_msg.robot_ef_corners_y = tracked_pose_msg.robot_ef_corners_y.tolist(
)
self.pose_tracking_pub.publish(tracked_pose_msg)
self.publish_image(frame_markers)
text = "Successfully Tracking " + str(len(output)) + " markers"
# rospy.loginfo(tracked_pose_msg.workpiece_corners_x)
# rospy.loginfo(text)
# print(output)
#
#
# for i in range(len(ids)):
# c = corners[i][0]
# center = (int(c[:, 0].mean()), int(c[:, 1].mean()))
# cv2.putText(frame_markers,str(i),center,cv2.FONT_HERSHEY_SIMPLEX,1,(255,0,0),10,bottomLeftOrigin=False)
# plt.plot([c[:, 0].mean()], [c[:, 1].mean()], "o", label = "id={0}".format(ids[i]))
# centers.append(center)
# self.assign_markers(centers)
return output
except Exception:
# pass
traceback.print_exc()
rospy.logerr('Not able to track')
#lists of ids and the corners beloning to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
for a in range(len(corners)):
target = corners[a]
if ids[a] == id_to_look_for:
center, height, distance = get_robot_positions(target)
height_list.append(height)
print("ID found")
#It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
gray = aruco.drawDetectedMarkers(gray, corners)
if show_video:
# Display the resulting frame
frame = aruco.drawDetectedMarkers(frame, corners)
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.stop()
cv2.destroyAllWindows()
if (len(height_list) == 0):
print("No targets Found")
raise SystemExit(0)
ppi.set_velocity(0, 0)
# get current frame
curr = ppi.get_image()
# visualise ARUCO marker detection annotations
aruco_params = aruco.DetectorParameters_create()
aruco_params.minDistanceToBorder = 0
aruco_params.adaptiveThreshWinSizeMax = 1000
aruco_dict = aruco.Dictionary_get(cv2.aruco.DICT_4X4_100)
corners, ids, rejected = aruco.detectMarkers(curr, aruco_dict, parameters=aruco_params)
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_length, camera_matrix, dist_coeffs)
aruco.drawDetectedMarkers(curr, corners, ids) # for detected markers show their ids
aruco.drawDetectedMarkers(curr, rejected, borderColor=(100, 0, 240)) # unknown squares
# scale to 144p
resized = cv2.resize(curr, (960, 720), interpolation = cv2.INTER_AREA)
# add GUI text
cv2.putText(resized, 'PenguinPi', (15, 50), font, font_scale, font_col, line_type)
# visualisation
cv2.imshow('video', resized)
cv2.waitKey(1)
# compute a marker's estimated pose and distance to the robot
if ids is None:
continue
#resizing using "resampling using pixel area relation" default is "INTER_LINEAR"- a bilinear interpolation
return cv2.resize(img, dim, interpolation=cv2.INTER_AREA)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
frame = cv2.imread("images/markerPic.jpg")
#frame=cv2.imread("calibrationImages/IMG_20191210_183704.jpg")
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
resized = scaleImage(gray, 10)
parameters = cv2.aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(resized,
aruco_dict,
parameters=parameters)
print(corners)
frame_markers = aruco.drawDetectedMarkers(resized.copy(), corners, ids)
#cv2.imshow('frame',frame_markers)
fig = plt.figure()
plt.imshow(frame_markers)
for i in range(len(ids)):
c = corners[i][
0] #corners is a list of lists, and the corner coordinatnes are in the first list
#then we plot the mean x and y value of the corners though this wont actually be the center
plt.plot([c[:, 0].mean()], [c[:, 1].mean()],
"o",
label="id={0}".format(ids[i]))
plt.legend()
plt.show()
cv2.waitKey(0)
def aruco_fun():
global xpos
global ypos
global alpha
global thetha
global xpos2
global ypos2
global alpha2
global thetha2
global xpos3
global ypos3
global alpha3
global thetha3
global p
global q
while (True):
# Capture frame-by-frame
ret, frame = cap.read()
#print(frame.shape) #480x640
# Our operations on the frame come here
gray = frame #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)
gray = aruco.drawDetectedMarkers(gray, corners, ids)
gray = cv2.circle(gray, ((int)(x), (int)(y)), 10, (0, 0, 255), -1)
gray = cv2.circle(gray, ((int)(x2), (int)(y2)), 10, (0, 255, 0), -1)
gray = cv2.circle(gray, ((int)(x3), (int)(y3)), 10, (255, 0, 0), -1)
gray = cv2.circle(gray, ((int)(p), (int)(q)), 10, (255, 255, 255), -1)
try:
if ids[0] == 1:
ID1 = 0
elif ids[1] == 1:
ID1 = 1
elif ids[2] == 1:
ID1 = 2
if ids[0] == 2:
ID2 = 0
elif ids[1] == 2:
ID2 = 1
elif ids[2] == 2:
ID2 = 2
if ids[0] == 3:
ID3 = 0
elif ids[1] == 3:
ID3 = 1
elif ids[2] == 3:
ID3 = 2
xpos = (corners[ID1][0][0][0] + corners[ID1][0][1][0] +
corners[ID1][0][2][0] + corners[ID1][0][3][0]) / 4
ypos = (corners[ID1][0][0][1] + corners[ID1][0][1][1] +
corners[ID1][0][2][1] + corners[ID1][0][3][1]) / 4
xpos2 = (corners[ID2][0][0][0] + corners[ID2][0][1][0] +
corners[ID2][0][2][0] + corners[ID2][0][3][0]) / 4
ypos2 = (corners[ID2][0][0][1] + corners[ID2][0][1][1] +
corners[ID2][0][2][1] + corners[ID2][0][3][1]) / 4
xpos3 = (corners[ID3][0][0][0] + corners[ID3][0][1][0] +
corners[ID3][0][2][0] + corners[ID3][0][3][0]) / 4
ypos3 = (corners[ID3][0][0][1] + corners[ID3][0][1][1] +
corners[ID3][0][2][1] + corners[ID3][0][3][1]) / 4
gray = cv2.circle(gray, ((int)(xpos), (int)(ypos)), 10,
(0, 0, 255), -1)
gray = cv2.circle(gray, ((int)(xpos2), (int)(ypos2)), 10,
(0, 255, 0), -1)
gray = cv2.circle(gray, ((int)(xpos3), (int)(ypos3)), 10,
(255, 0, 0), -1)
if corners[ID1][0][0][1] <= corners[ID1][0][1][1]:
if corners[ID1][0][0][0] <= corners[ID1][0][1][0]:
thetha = (-math.degrees(
math.atan(
(corners[ID1][0][0][1] - corners[ID1][0][3][1]) /
(corners[ID1][0][0][0] - corners[ID1][0][3][0]))))
else:
thetha = (360 - math.degrees(
math.atan(
(corners[ID1][0][0][1] - corners[ID1][0][3][1]) /
(corners[ID1][0][0][0] - corners[ID1][0][3][0]))))
else:
if corners[ID1][0][0][0] <= corners[ID1][0][1][0]:
thetha = (180 - math.degrees(
math.atan(
(corners[ID1][0][0][1] - corners[ID1][0][3][1]) /
(corners[ID1][0][0][0] - corners[ID1][0][3][0]))))
else:
thetha = (180 - math.degrees(
math.atan(
(corners[ID1][0][0][1] - corners[ID1][0][3][1]) /
(corners[ID1][0][0][0] - corners[ID1][0][3][0]))))
if xpos <= x:
if ypos >= y:
alpha = math.degrees(math.atan((ypos - y) / (x - xpos)))
else:
alpha = (360 +
math.degrees(math.atan((ypos - y) / (x - xpos))))
else:
alpha = (180 + math.degrees(math.atan(
(ypos - y) / (x - xpos))))
if corners[ID2][0][0][1] <= corners[ID2][0][1][1]:
if corners[ID2][0][0][0] <= corners[ID2][0][1][0]:
thetha2 = (-math.degrees(
math.atan(
(corners[ID2][0][0][1] - corners[ID2][0][3][1]) /
(corners[ID2][0][0][0] - corners[ID2][0][3][0]))))
else:
thetha2 = (360 - math.degrees(
math.atan(
(corners[ID2][0][0][1] - corners[ID2][0][3][1]) /
(corners[ID2][0][0][0] - corners[ID2][0][3][0]))))
else:
if corners[ID2][0][0][0] <= corners[ID2][0][1][0]:
thetha2 = (180 - math.degrees(
math.atan(
(corners[ID2][0][0][1] - corners[ID2][0][3][1]) /
(corners[ID2][0][0][0] - corners[ID2][0][3][0]))))
else:
thetha2 = (180 - math.degrees(
math.atan(
(corners[ID2][0][0][1] - corners[ID2][0][3][1]) /
(corners[ID2][0][0][0] - corners[ID2][0][3][0]))))
if xpos2 <= x2:
if ypos2 >= y2:
alpha2 = math.degrees(
math.atan((ypos2 - y2) / (x2 - xpos2)))
else:
alpha2 = (
360 +
math.degrees(math.atan((ypos2 - y2) / (x2 - xpos2))))
else:
alpha2 = (180 +
math.degrees(math.atan((ypos2 - y2) / (x2 - xpos2))))
if corners[ID3][0][0][1] <= corners[ID3][0][1][1]:
if corners[ID3][0][0][0] <= corners[ID3][0][1][0]:
thetha3 = (-math.degrees(
math.atan(
(corners[ID3][0][0][1] - corners[ID3][0][3][1]) /
(corners[ID3][0][0][0] - corners[ID3][0][3][0]))))
else:
thetha3 = (360 - math.degrees(
math.atan(
(corners[ID3][0][0][1] - corners[ID3][0][3][1]) /
(corners[ID3][0][0][0] - corners[ID3][0][3][0]))))
else:
if corners[ID3][0][0][0] <= corners[ID3][0][1][0]:
thetha3 = (180 - math.degrees(
math.atan(
(corners[ID3][0][0][1] - corners[ID3][0][3][1]) /
(corners[ID3][0][0][0] - corners[ID3][0][3][0]))))
else:
thetha3 = (180 - math.degrees(
math.atan(
(corners[ID3][0][0][1] - corners[ID3][0][3][1]) /
(corners[ID3][0][0][0] - corners[ID3][0][3][0]))))
if xpos3 <= x3:
if ypos3 >= y3:
alpha3 = math.degrees(
math.atan((ypos3 - y3) / (x3 - xpos3)))
else:
alpha3 = (
360 +
math.degrees(math.atan((ypos3 - y3) / (x3 - xpos3))))
else:
alpha3 = (180 +
math.degrees(math.atan((ypos3 - y3) / (x3 - xpos3))))
'''print("x2=",xpos2)
print("y2=",ypos2)
print("x3=",xpos3)
print("y3=",ypos3)'''
#It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
#gray = aruco.drawDetectedMarkers(gray, corners, ids)
#print(rejectedImgPoints)
# Display the resulting frame
#gray=cv2.circle(gray,((int)(xpos),(int)(ypos)),10, (0,0,255), -1)
except:
pass
cv2.imshow('frame', gray)
if (a_reached == 1) & (b_reached == 1) & (c_reached == 1):
pso()
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
net = cv2.dnn.readNetFromCaffe(protoFile, weightsFile)
#-------------------------------------------------------------------------------------------------------------------------------------
while (True):
hasFrame, frame = cap.read()
gray = cv2.cvtColor(
frame, cv2.COLOR_BGR2GRAY) #grayscale conversion of existing frame
#-----------------ARUCO MARKER DETECTION---------------------------------------------
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
parameters = aruco.DetectorParameters_create()
corners, aruco_id, _ = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
image1 = aruco.drawDetectedMarkers(frame.copy(), corners, aruco_id)
#------------------------------------------------------------------------------------
frame = cv2.flip(frame, 1) #flipping the frame
frameCopy = np.copy(frame) #creating a copy of the frame
subtracted = frame - frame
roi = frame - frame
if not hasFrame:
cv2.waitKey()
break
#------------------------------------------------------------------------------------
frameWidth = frame.shape[1]
frameHeight = frame.shape[0]
def plot_analysis_result(self):
if self.corners is not None:
self.img = aruco.drawDetectedMarkers(self.img, self.corners)
cv.circle(self.img, self.center, 2, (0, 0, 255), 2)
