def augment_reality(path):
#Load reference image and replacement image
replacement_img = cv2.imread(path)
board_img = cv2.imread("board_aruco.png")
#Detect markers in reference image and get origin points
board_img_gray = cv2.cvtColor(board_img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
board_markers, board_ids, _ = aruco.detectMarkers(board_img_gray, aruco_dict, parameters=parameters)
orig_points = np.asarray(board_markers).reshape((-1,2))
# Using webcam
cap = cv2.VideoCapture(0)
while(True):
ret, frame = cap.read() #640x480
frame_copy = frame.copy()
# Detecting markers in real time
gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
markers, ids, _ = aruco.detectMarkers(gray_image, aruco_dict, parameters=parameters)
if (len(markers) == 0):
print("Nenhum marcador encontrado.")
else:
dest_points = np.asarray(markers).reshape((-1,2))
# Find the reference coordinate of the detected markers
ids = np.asarray(ids)
detected_orig_points = []
for id in ids:
index = np.argwhere(board_ids == id)[0][0]
for i in range(4):
detected_orig_points.append(orig_points[(index*4)+i])
detected_orig_points = np.asarray(detected_orig_points).astype(int)
# Black image with replacement image over markers
homography = cv2.findHomography(detected_orig_points, dest_points)[0]
h, w, _ = frame.shape
warped_image = cv2.warpPerspective(replacement_img, homography, (w, h), frame_copy)
# Optional -- draw detected markers in image
# aruco.drawDetectedMarkers(frame, markers)
# Overlapping images
mask = warped_image > [0, 0, 0]
frame = np.where(mask == True, warped_image, frame)
# Show image, press q to exit
cv2.imshow('clean image', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Clean everything up
cap.release()
cv2.destroyAllWindows()
def ARtag_Detection(self):
if not self.camera_open:
self.openCamera()
print "Detecting AR tags..."
currentImage = self.getCurrentImage()
imageData = currentImage.data
imageData = numpy.reshape(imageData, (800, 1280, 4))
gray = cv2.cvtColor(imageData, cv2.COLOR_BGRA2GRAY)
corners, ids, rejected = aruco.detectMarkers(gray, self._aruco_dict, parameters=self._arucoParams)
if ids is not None:
Tmat = []
IDS = []
detectioninfo = RR.RobotRaconteurNode.s.NewStructure("BaxterCamera_interface.ARtagInfo")
for anid in ids:
IDS.append(anid[0])
for corner in corners:
pc, Rc = self.getObjectPose(corner)
Tmat.extend([ Rc[0][0], Rc[1][0], Rc[2][0], 0.0,
Rc[0][1], Rc[1][1], Rc[2][1], 0.0,
Rc[0][2], Rc[1][2], Rc[2][2], 0.0,
pc[0], pc[1], pc[2], 1.0])
detectioninfo.tmats = Tmat
detectioninfo.ids = IDS
return detectioninfo
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 getPose(im, board, m, d):
if len(im.shape) > 2:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
else:
gray = im
# parameters = aruco.DetectorParameters_create()
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
ret, rvecs, tvecs = aruco.estimatePoseCharucoBoard(chcorners, chids, board,m,d)
im = aruco.drawAxis(im, m, d, rvecs, tvecs,0.5)
cv2.imshow('markers', im)
cv2.waitKey()
def analyze_image(self):
gray = cv.cvtColor(self.img, cv.COLOR_RGB2GRAY)
all_corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters)
if all_corners:
self.corners = all_corners
corners = all_corners[0][0]
dim = corners.shape
c = np.sum(corners, axis=0) / dim[0]
self.center = (c[0], c[1])
x1, y1 = corners[0][0], corners[0][1]
x2, y2 = corners[1][0], corners[1][1]
x3, y3 = corners[2][0], corners[2][1]
x4, y4 = corners[3][0], corners[3][1]
# A = (1 / 2) | [(x3 - x1)(y4 - y2) + (x4 - x2)(y1 - y3)] |
self.marker_size = math.sqrt(abs((x3 - x1) * (y4 - y2) + (x4 - x2) * (y1 - y3)) / 2)
else:
self.corners = None
self.center = (-1, -1)
self.marker_size = 0
def detect():
# camera: c270
cameraMatrix = np.array([[8.2177218160147447e+02, 0.0 , 3.4694289806342221e+02],
[0.0 , 8.2177218160147447e+02, 2.4795144956871457e+02],
[0.0 , 0.0 , 1.0 ]])
distCoeffs = np.array([4.4824308523616324e-02, -7.4951985348854000e-01, 3.7539708742088725e-03, 8.8931335565222442e-03, 3.7214188475390984e+00])
rvecs = np.array([])
tvecs = np.array([])
ret, frame = cap.read()
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(gray, aruco_dict, parameters=parameters)
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
while (True):
# 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_ARUCO_ORIGINAL)
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 ids, type(ids)
print ids, type(ids)
print corners[0], type(corners[0])
#print corners[0], type(corners[0])
for marker in range(len(ids)):
print ids.item(marker)
print np.array(corners[marker].tolist())
print "shape", corners[marker].shape
for fourpoints in range(4):
print corners[marker].item(0, fourpoints, 0), corners[marker].item(
0, fourpoints, 1)
print
print "------"
#It's working.
def track(self, matrix_coefficients, distortion_coefficients):
global pressedKey, needleComposeRvec, needleComposeTvec, ultraSoundComposeRvec, ultraSoundComposeTvec
""" Real time ArUco marker tracking. """
TcomposedRvecNeedle, TcomposedTvecNeedle = None, None
TcomposedRvecUltrasound, TcomposedTvecUltrasound = None, None
# Behaviour is a key between calibration types.
# No simulation is equal to 0
# Needle Calibration is equal to 1
# Ultrasound Calibration is equal to 2
# Press
behaviour = 0
try:
while True:
# No marker detected
isCalibrationMarkerDetected = False
isNeedleDetected = False
isUltraSoundDetected = False
# Get the next frame to process
ret, frame = cap.read()
# operations on the frame come here
gray = cv2.cvtColor(frame,
cv2.COLOR_BGR2GRAY) # Change grayscale
aruco_dict = aruco.Dictionary_get(
aruco.DICT_5X5_250) # Use 5x5 dictionary to find markers
parameters = aruco.DetectorParameters_create(
) # Marker detection parameters
# lists of ids and the corners beloning to each id
corners, ids, rejected_img_points = aruco.detectMarkers(
gray,
aruco_dict,
parameters=parameters,
cameraMatrix=matrix_coefficients,
distCoeff=distortion_coefficients)
# Get the behaviour and update the gui
if behaviour == 0:
self.calibrationType.emit('Simulation')
elif behaviour == 1:
self.calibrationType.emit('Needle calibration')
else:
self.calibrationType.emit('Ultrasound calibration')
pass
if np.all(ids is not None
): # If there are markers found by detector
# sort the markers
zipped = zip(ids, corners)
ids, corners = zip(*(sorted(zipped)))
# 4 axis for ultrasound, 2axis for needle
axisForFourPoints = np.float32([[-0.02, -0.02, 0],
[-0.02, 0.02, 0],
[0.02, -0.02, 0],
[0.02, 0.02, 0]]).reshape(
-1,
3) # axis for a plan
axisForTwoPoints = np.float32([[0.01, 0, 0],
[-0.01, 0, 0]]).reshape(
-1,
3) # axis for a line
for i in range(0, len(ids)): # Iterate in markers
# Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(
corners[i], 0.02, matrix_coefficients,
distortion_coefficients)
if ids[i] == calibrationMarkerID:
calibrationRvec = rvec
calibrationTvec = tvec
isCalibrationMarkerDetected = True
calibrationMarkerCorners = corners[i]
elif ids[i] == needleMarkerID:
needleRvec = rvec
needleTvec = tvec
isNeedleDetected = True
needleCorners = corners[i]
elif ids[i] == ultraSoundMarkerID:
ultraSoundRvec = rvec
ultraSoundTvec = tvec
isUltraSoundDetected = True
ultrasoundCorners = corners[i]
(rvec - tvec).any(
) # get rid of that nasty numpy value array error
# aruco.drawAxis(frame, matrix_coefficients, distortion_coefficients, rvec, tvec, 0.01)
frame = aruco.drawDetectedMarkers(
frame, corners) # Draw A square around the markers
if isNeedleDetected and needleComposeRvec is not None and needleComposeTvec is not None:
info = cv2.composeRT(needleComposeRvec,
needleComposeTvec, needleRvec.T,
needleTvec.T)
TcomposedRvecNeedle, TcomposedTvecNeedle = info[
0], info[1]
imgpts, jac = cv2.projectPoints(
axisForTwoPoints, TcomposedRvecNeedle,
TcomposedTvecNeedle, matrix_coefficients,
distortion_coefficients)
frame = draw(frame, imgpts, (200, 200, 220))
if isUltraSoundDetected and ultraSoundComposeRvec is not None and ultraSoundComposeTvec is not None:
info = cv2.composeRT(ultraSoundComposeRvec,
ultraSoundComposeTvec,
ultraSoundRvec.T,
ultraSoundTvec.T)
TcomposedRvecUltrasound, TcomposedTvecUltrasound = info[
0], info[1]
imgpts, jac = cv2.projectPoints(
axisForFourPoints, TcomposedRvecUltrasound,
TcomposedTvecUltrasound, matrix_coefficients,
distortion_coefficients)
frame = draw(frame, imgpts, (60, 200, 50))
# If the both markers can be seen by the camera, print the xyz differences between them
# So it will guide the user
if isNeedleDetected and needleComposeRvec is not None and needleComposeTvec is not None and \
isUltraSoundDetected and ultraSoundComposeRvec is not None and ultraSoundComposeTvec is not None:
xdiff = round((TcomposedTvecNeedle[0] -
TcomposedTvecUltrasound[0])[0], 3)
ydiff = round((TcomposedTvecNeedle[1] -
TcomposedTvecUltrasound[1])[0], 3)
zdiff = round((TcomposedTvecNeedle[2] -
TcomposedTvecUltrasound[2])[0], 3)
self.changeXDiff.emit('X difference:' + str(xdiff))
self.changeYDiff.emit('Y difference:' + str(ydiff))
self.changeZDiff.emit('Z difference:' + str(zdiff))
# Display the resulting frame
rgbImage = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
convertToQtFormat = QImage(rgbImage.data, rgbImage.shape[1],
rgbImage.shape[0],
QImage.Format_RGB888)
p = convertToQtFormat.scaled(640, 480, Qt.KeepAspectRatio)
self.changePixmap.emit(p)
# Key handling. For marker calibration, saving marker etc.
if pressedKey is None: # No key pressed
pass
elif pressedKey == Qt.Key_C: # Button for calibration.
if ids is not None and len(
ids
) > 1: # If there are two markers, reverse the second and get the difference
if isNeedleDetected and isCalibrationMarkerDetected and behaviour == 1:
needleComposeRvec, needleComposeTvec = relativePosition(
calibrationRvec, calibrationTvec, needleRvec,
needleTvec)
savedNeedleRvec, savedNeedleTvec = needleComposeRvec, needleComposeTvec
elif isUltraSoundDetected and isCalibrationMarkerDetected and behaviour == 2:
ultraSoundComposeRvec, ultraSoundComposeTvec = relativePosition(
calibrationRvec, calibrationTvec,
ultraSoundRvec, ultraSoundTvec)
savedUltraSoundRvec, savedUltraSoundTvec = ultraSoundComposeRvec, ultraSoundComposeTvec
elif pressedKey == Qt.Key_U: # Button for moving z axis 1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[0], [0],
[0.001]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
0
], [0], [0.001]]
elif pressedKey == Qt.Key_D: # Button for moving z axis -1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[0], [0],
[-0.001]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
0
], [0], [-0.001]]
elif pressedKey == Qt.Key_R: # Button for moving x axis 1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[0.001], [0],
[0]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
0.001
], [0], [0]]
elif pressedKey == Qt.Key_L: # Button for moving x axis -1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[-0.001], [0],
[0]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
-0.001
], [0], [0]]
elif pressedKey == Qt.Key_B: # Button for moving y axis -1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[0], [-0.001],
[0]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
0
], [-0.001], [0]]
elif pressedKey == Qt.Key_F: # Button for moving y axis 1 mm
if behaviour == 1 and needleComposeTvec is not None:
needleComposeTvec = needleComposeTvec + [[0], [0.001],
[0]]
elif behaviour == 2 and ultraSoundComposeTvec is not None:
ultraSoundComposeTvec = ultraSoundComposeTvec + [[
0
], [0.001], [0]]
elif pressedKey == Qt.Key_P: # print necessary information here, for debug
pass # Insert necessary print here
elif pressedKey == Qt.Key_S: # Save the calibration vectors to a file.
if (needleComposeRvec is not None
and needleComposeTvec is not None
and ultraSoundComposeRvec is not None
and ultraSoundComposeTvec is not None):
print(needleComposeRvec, needleComposeTvec,
ultraSoundComposeRvec, ultraSoundComposeTvec)
fileObject = open(save_Name, 'wb')
data = [
needleComposeRvec, needleComposeTvec,
ultraSoundComposeRvec, ultraSoundComposeTvec
]
pickle.dump(data, fileObject)
fileObject.close()
elif pressedKey == Qt.Key_X: # change simulation type, "Simulation, needle calibration, ultrasound calibration"
behaviour = (behaviour + 1) % 3
# print(behaviour)
pressedKey = None
except Exception:
pass
finally:
# When everything done, release the capture
cap.release()
return frame
# Capture frame-by-frame
(ret, frame) = cap.read()
# frame = frame[300:640,100:560]
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
cv2.imshow('original', frame)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
cv2.imshow('grayed', gray)
# ret3,gray = cv2.threshold(gray,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
# cv2.imshow("thresholded", gray)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=arucoParams) # Detect aruco
aruco.refineDetectedMarkers(gray, board, corners, ids, rejectedImgPoints)
if ids is not None: # if the aruco marker detected
# rvec, tvec, objpoints = aruco.estimatePoseSingleMarkers(corners, markerLength, camera_Matrix, dist_Coff) # For a single marker
# board = aruco.Board_create(objpoints,dictionary,ids)
imgWithAruco = aruco.drawDetectedMarkers(frame, corners, ids,
(0, 255, 0))
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_Matrix, dist_Coff)
imgWithAruco = aruco.drawAxis(frame, camera_Matrix, dist_Coff, rvec,
tvec, 0.1)
# imgWithArucwo = aruco.drawAxis(imgWithAruco, camera_Matrix, dist_Coff, rvec, tvec, 10) #balash weghet nazar abo balash katar meno # axis length 100 can be changed according to your requirement
x_dist = round(tvec[0][0] * 100, 1)
y_dist = round(tvec[1][0] * 100, 1)
R_flip[2, 2] = -1.0
#--- start imutils fps counter
fps = FPS().start()
#--- LOOP - capture frames from the camera
for frame_pi in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
frame = frame_pi.array
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#-- Find all the aruco markers in the image
corners, ids, rejected = aruco.detectMarkers(image=frame,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
if ids is not None:
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix, camera_distortion)
rvec, tvec = ret[0], ret[1]
aruco.drawDetectedMarkers(frame, corners)
i = 0
for id in ids_to_find:
id_pos = np.where(ids == id)
def detect_markers(self, frame) -> ArucoDetections:
corners, ids, rejected = aruco.detectMarkers(
frame, dictionary=self._aruco_dict)
return ArucoDetections(corners, ids, rejected)
def callback(status):
#agv_stopped = status.data
print('Received status from AGV')
sub = rospy.Subscriber('/AGV_status', Float64MultiArray, callback)
while True:
# Saves images from the video stream and converts it into a grayscale image
# ret is currently unused (check if removable)
ret, frame = video_stream.read()
gray_img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Get the corner positions of each (visible) marker in the image
corners, ids, rejected = aruco.detectMarkers(image=gray_img, dictionary=aruco_dict, parameters=detector_parameters)
if (ids is not None):
for i in range(0, len(ids)):
# Calculates the markers positions and places each position in an array
result = aruco.estimatePoseSingleMarkers(corners[i], marker_size, calibMatrix, distMatrix)
rvec, tvec = result[0][0,0,:], result[1][0,0,:]
#print("Marker found, printing coordinates for id = " + str(ids[i]))
#print("x-position = ", round(tvec[0], 1), ", y-position = ", round(tvec[1], 1), ", z-position = ", round(tvec[2], 1))
aruco.drawDetectedMarkers(frame, corners)
#aruco.drawAxis(frame, calibMatrix, distMatrix, rvec, tvec, 10)
def roda_todo_frame(imagem):
global centro_yellow
global m
global angle_yellow
global creeper_vermelho
global creeper_verde
global creeper_azul
global dic_creepers
global ids
global id_encontrado
try:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
##
copia = cv_image.copy() # se precisar usar no while
if frame % skip == 0: # contamos a cada skip frames
mask = projeto_utils.filter_color(copia, low, high)
img, centro_yellow = projeto_utils.center_of_mass_region(
mask, 200, 300, mask.shape[1], mask.shape[0])
#----------------------------------------- CREEPERS ----------------------------------------#
creeper_vermelho, creeper_verde, creeper_azul = projeto_utils.identifica_creeper(
cv_image)
dic_creepers = {
'azul': creeper_azul,
'vermelho': creeper_vermelho,
'verde': creeper_verde
}
#-------------------------------------------------------------------------------------------#
#----------------------------------------- REGRESSAO ---------------------------------------#
saida_bgr, m, h = projeto_utils.ajuste_linear_grafico_x_fy(mask)
ang = math.atan(m)
ang_deg = math.degrees(ang)
angle_yellow = ang_deg
#-------------------------------------------------------------------------------------------#
#----------------------------------------- ARUCO -------------------------------------------#
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# verificando se o id encontrado é o desejado
try:
for id in ids:
if id_to_find == int(id[0]):
id_encontrado = True
except:
pass
######################################## ARUCO #############################################
if ids is not None:
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(cv_image, corners, ids)
aruco.drawAxis(cv_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(cv_image, str_position, (0, 100), font, 1,
(0, 255, 0), 1, cv2.LINE_AA)
#####################---- 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(cv_image, str_dist, (0, 15), font, 1, (0, 255, 0),
1, cv2.LINE_AA)
#####################---- Distancia pelo foco ----#####################
# raspicam v2 focal legth
FOCAL_LENGTH = 3.6 #3.04
# pixel por unidade de medida
m = (camera_matrix[0][0] / FOCAL_LENGTH +
camera_matrix[1][1] / FOCAL_LENGTH) / 2
# corners[0][0][0][0] = [ID][plano?][pos_corner(sentido horario)][0=valor_pos_x, 1=valor_pos_y]
pixel_length1 = math.sqrt(
math.pow(corners[0][0][0][0] - corners[0][0][1][0], 2) +
math.pow(corners[0][0][0][1] - corners[0][0][1][1], 2))
pixel_length2 = math.sqrt(
math.pow(corners[0][0][2][0] - corners[0][0][3][0], 2) +
math.pow(corners[0][0][2][1] - corners[0][0][3][1], 2))
pixlength = (pixel_length1 + pixel_length2) / 2
dist = marker_size * FOCAL_LENGTH / (pixlength / m)
#-- Print distancia focal
str_distfocal = "Dist focal=%4.0f" % (dist)
print(str_distfocal)
cv2.putText(cv_image, str_distfocal, (0, 30), font, 1,
(0, 255, 0), 1, cv2.LINE_AA)
####################--------- desenha o cubo -----------#########################
m = marker_size / 2
pts = np.float32([[-m, m, m], [-m, -m, m], [m, -m,
m], [m, m, m],
[-m, m, 0], [-m, -m, 0], [m, -m, 0],
[m, m, 0]])
imgpts, _ = cv2.projectPoints(pts, rvec, tvec, camera_matrix,
camera_distortion)
imgpts = np.int32(imgpts).reshape(-1, 2)
cv_image = cv2.drawContours(cv_image, [imgpts[:4]], -1,
(0, 0, 255), 4)
for i, j in zip(range(4), range(4, 8)):
cv_image = cv2.line(cv_image, tuple(imgpts[i]),
tuple(imgpts[j]), (0, 0, 255), 4)
cv_image = cv2.drawContours(cv_image, [imgpts[4:]], -1,
(0, 0, 255), 4)
#-------------------------------------------------------------------------------------------#
projeto_utils.texto(cv_image,
f"Distancia obstaculo: {distancia}", (15, 50),
color=(0, 0, 255))
cv2.imshow("Camera", cv_image)
cv2.imshow("Creeper", dic_creepers[cor])
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def run(self):
cap, resolution = init_cv()
t = threading.current_thread()
cf, URI = init_cf()
if cf is None:
print('Not running cf code.')
return
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
font = cv2.FONT_HERSHEY_PLAIN
id2find = [0, 1]
marker_size = [0.112, 0.0215] # 0.1323
ids_seen = [0, 0]
id2follow = 0
zvel = 0.0
landMode = False
camera_matrix, camera_distortion, _ = loadCameraParams('runcam_nano3')
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
with open("ugly_log.txt", "a") as file:
lgr = UglyLogger(URI, scf, file)
# We take off when the commander is created
with MotionCommander(scf) as mc:
mc.up(0.1, 0.5)
while not self._stopevent.isSet():
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(
image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
if ids is not None and len(ids) > 0:
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
#-- Calculate which marker has been seen most at late
if 0 in ids:
ids_seen[0] += 1
else:
ids_seen[0] = 0
if 1 in ids:
ids_seen[1] += 2
else:
ids_seen[1] = 0
id2follow = np.argmax(ids_seen)
idx_r, idx_c = np.where(ids == id2follow)
#-- Extract the id to follow
corners = np.asarray(corners)
corners = corners[idx_r, :]
#-- Estimate poses of detected markers
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, marker_size[id2follow], camera_matrix,
camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = rvecs[0, 0, :], tvecs[0, 0, :]
# Draw the detected markers axis
for i in range(len(rvecs)):
aruco.drawAxis(frame, camera_matrix,
camera_distortion,
rvecs[i, 0, :], tvecs[i, 0, :],
marker_size[id2follow] / 2)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
#-- Now get Position and attitude of the camera respect to the marker
if id2follow == 0:
pos_camera = -R_tc * (np.matrix(tvec).T -
T_slide)
else:
pos_camera = -R_tc * (np.matrix(tvec).T)
str_position = "Position error: x=%4.4f y=%4.4f z=%4.4f" % (
pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 20), font, 1,
uglyConst.BLACK, 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
roll_camera, pitch_camera, yaw_camera = rotationMatrixToEulerAngles(
R_flip * R_tc)
att_camera = [
math.degrees(roll_camera),
math.degrees(pitch_camera),
math.degrees(yaw_camera)
]
str_attitude = "Anglular error: roll=%4.4f pitch=%4.4f yaw (z)=%4.4f" % (
att_camera[0], att_camera[1], att_camera[2])
cv2.putText(frame, str_attitude, (0, 40), font, 1,
uglyConst.BLACK, 2, cv2.LINE_AA)
drawHUD(frame, resolution, yaw_camera)
pos_flip = np.array([[-pos_camera.item(1)],
[pos_camera.item(0)]])
cmd_flip = np.array(
[[np.cos(yaw_camera), -np.sin(yaw_camera)],
[np.sin(yaw_camera),
np.cos(yaw_camera)]])
pos_cmd = cmd_flip.dot(
pos_flip) # cmd_flip*pos_flip
print('Following tag ', id2follow,
' with position ', pos_cmd[0], pos_cmd[1])
flowHeight = lgr.getHeight()
print('Height is', flowHeight)
#-- If far away or big angle, move/turn closer at constant height
if (np.sqrt(pos_cmd[0] * pos_cmd[0] +
pos_cmd[1] * pos_cmd[1]) > 0.05) or (
np.abs(pos_camera.item(2) > 1.0)):
print('1. Far away, moving closer')
mc._set_vel_setpoint(
pos_cmd[0] * uglyConst.Kx,
pos_cmd[1] * uglyConst.Ky, 0.0,
-att_camera[2] * uglyConst.Kyaw)
#-- If on the ground, "maintain" height and adjust
elif flowHeight < 40:
print('4. Landed')
mc._set_vel_setpoint(
pos_cmd[0] * uglyConst.Kx,
pos_cmd[1] * uglyConst.Ky, -0.2,
-att_camera[2] * uglyConst.Kyaw)
#-- If really close, shut down motors
if (np.sqrt(pos_cmd[0] * pos_cmd[0] +
pos_cmd[1] * pos_cmd[1]) < 0.01):
mc.stop()
print('5. Close!')
break
#-- If close to the ground and ready 2 land, move down faster
elif flowHeight < 200 and (
np.sqrt(pos_cmd[0] * pos_cmd[0] +
pos_cmd[1] * pos_cmd[1]) > 0.01):
mc.stop()
break
print(
'3. Within ground effect, moving down fast'
)
mc._set_vel_setpoint(
pos_cmd[0] * uglyConst.Kx,
pos_cmd[1] * uglyConst.Ky, -0.2,
-att_camera[2] * uglyConst.Kyaw)
#-- Otherwise, move down
else:
print('2. Close, moving down')
mc._set_vel_setpoint(
pos_cmd[0] * uglyConst.Kx,
pos_cmd[1] * uglyConst.Ky, -0.05,
-att_camera[2] * uglyConst.Kyaw)
landMode = True
cv2.imshow('frame', frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cap.release()
cv2.destroyAllWindows()
elif key == ord('w'):
zvel += 0.1
elif key == ord('s'):
zvel -= 0.1
# We land when the commander goes out of scope
print('Landing...')
print('Stopping cf_thread')
def run(self):
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
font = cv2.FONT_HERSHEY_PLAIN
camera_matrix, camera_distortion, _ = loadCameraParams('webcam')
cap, resolution = init_cv()
while True:
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(
image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
if ids is not None:
#-- Estimate poses of detected markers
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, marker_size, camera_matrix, camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = rvecs[0, 0, :], tvecs[0, 0, :]
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
# Draw the detected markers axis
for i in range(len(rvecs)):
aruco.drawAxis(frame, camera_matrix, camera_distortion,
rvecs[i, 0, :], tvecs[i, 0, :], 0.1)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
#-- Now get Position and attitude of the camera respect to the marker
pos_camera = -R_tc * np.matrix(tvec).T
str_position = "Position error: x=%4.4f y=%4.4f z=%4.4f" % (
pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 20), font, 1,
uglyConst.BLACK, 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
roll_camera, pitch_camera, yaw_camera = rotationMatrixToEulerAngles(
R_flip * R_tc)
str_attitude = "Anglular error: roll=%4.4f pitch=%4.4f yaw (z)=%4.4f" % (
math.degrees(roll_camera), math.degrees(pitch_camera),
math.degrees(yaw_camera))
cv2.putText(frame, str_attitude, (0, 40), font, 1,
uglyConst.BLACK, 2, cv2.LINE_AA)
drawHUD(frame, resolution, yaw_camera)
self.ctrl_message.errorx = pos_camera[0]
self.ctrl_message.errory = pos_camera[1]
self.ctrl_message.errorz = pos_camera[2]
self.ctrl_message.erroryaw = math.degrees(yaw_camera)
cv2.imshow('frame', frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cap.release()
cv2.destroyAllWindows()
def collect_charuco_detections(capture, aruco_dict, charuco_board):
# Create the arrays and variables we'll use to store info like corners and IDs from images processed
corners_all = [] # Corners discovered in all images processed
ids_all = [] # Aruco ids corresponding to corners discovered
image_size = None # Determined at runtime
good = 0
def show(frame):
# resize
proportion = max(frame.shape) / 800.0
im = cv2.resize(frame, (int(
frame.shape[1] / proportion), int(frame.shape[0] / proportion)))
# add text
txt = f'good frames so far: {good}'
im = cv2.putText(im, txt, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1,
(255, 0, 0), 2, cv2.LINE_AA)
# show the debug image
cv2.imshow('calibration', im)
while True:
more, img = capture.read()
if not more:
break
# Grayscale the image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
# Outline the aruco markers found in our query image
img = aruco.drawDetectedMarkers(image=img, corners=corners)
try:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=charuco_board)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if response > 20:
# Add these corners and ids to our calibration arrays
corners_all.append(charuco_corners)
ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
img = aruco.drawDetectedCornersCharuco(
image=img,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
# If our image size is unknown, set it now
if not image_size:
image_size = gray.shape[::-1]
good += 1
show(img)
else:
show(img)
except cv2.error as e:
show(img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
return corners_all, ids_all, image_size
def Image_Process(self):
ret = self.frame
gray = cv2.cvtColor(self.frame,
cv2.COLOR_BGR2GRAY) # 영상을 grayscale로 변환
# 카메라 파라미터, grayscale이미지, dictionary와 파라미터를 참고하여 마커를 찾는다. 모서리와 마커 id 반환 (rejected?)
corners, ids, rejected = aruco.detectMarkers(image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=cam_mat,
distCoeff=cam_distort)
# 마커가 검출될 경우
if ids is not None: #and self.freeze is not True:
self.cant_find_mark = False
# corner, 마커 크기, 카메라 파라미터를 이용하여 마커의 pose를 알아낸다
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
cam_mat, cam_distort)
rvec, tvec = ret[0][0, 0, :], ret[1][
0, 0, :] # 알아낸 pose 행렬을 자세, 위치 벡터로 변환
aruco.drawDetectedMarkers(self.frame,
corners) # 코너를 이용하여 frame에 마커를 그린다
aruco.drawAxis(
self.frame, cam_mat, cam_distort, rvec, tvec, 10
) # 카메라 파라미터, 마커 위치 및 자세 벡터를 이용하여 frame 이미지에 10 길이의 xyz축을 그린다.
str_position = "MARKER Position x=%4.0f y=%4.0f z=%4.0f" % (
tvec[0], tvec[1], tvec[2]) # 카메라 기준 마커의 위치 좌표
cv2.putText(self.frame, str_position, (0, 100), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
R_ct = np.matrix(
cv2.Rodrigues(rvec)[0]) # 로드리그스 행렬을 이용하여 벡터를 회전 행렬 변환
R_tc = R_ct.T
roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(
R_flip * R_tc) # 오일러 행렬로 변환
str_attitude = "MARKER Attitude r=%4.0f p=%4.0f y=%4.0f" % (
math.degrees(roll_marker), math.degrees(pitch_marker),
math.degrees(yaw_marker)) # 마커의 roll, pitch, yaw
cv2.putText(self.frame, str_attitude, (0, 150), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
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(self.frame, str_position, (0, 200), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
# 마커 기준 카메라의 자세는 카메라 기준 마커의 자세와 같다
str_attitude = "CAMERA Attitude r=%4.0f p=%4.0f y=%4.0f" % (
math.degrees(roll_marker), math.degrees(pitch_marker),
math.degrees(yaw_marker))
cv2.putText(self.frame, str_attitude, (0, 250), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
self.CAM_X = pos_camera[0]
self.CAM_Z = pos_camera[2]
self.CAM_PITCH = math.degrees(pitch_camera)
else: # 마커가 검출되지 않았을 경우
self.cant_find_mark = True
cv2.putText(self.frame, "can't find any marker", (0, 100), font, 1,
(255, 0, 0), 2, cv2.LINE_AA)
def callbackImage(self,data):
global out, ids
try:
src_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
(rows,cols,channels) = src_image.shape
except CvBridgeError as e:
print(e)
#-- 180 deg rotation matrix around x axis
R_flip = np.zeros((3,3), dtype=np.float)
R_flip[0,0] = +1.0
R_flip[1,1] = -1.0
R_flip[2,2] = -1.0
#-- Convert in gray scale\n",
gray = cv2.cvtColor(src_image, cv2.COLOR_BGR2GRAY) #-- remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image\n",
corners, ids, rejected = aruco.detectMarkers(image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
#rospy.loginfo("ids[]: %f", len(ids[0]))
#int(ids) == id_to_find:
# out = np.logical_and(ids != None, ids[0] == id_to_find)
# rospy.loginfo(out)
if ids != None:
#rospy.loginfo("Aruco working!")
if ids[0] == id_to_find:
#if (int(ids[0]) != None and int(ids[0]) == id_to_find):
#-- ret= [rvec,tvec, ?]
#-- array of rotation and position of each marker in camera frame
#-- rvec = [[rvec_1, [rvec2], ...]] attitude of the marker respect to camera frame
#-- tvec = [[tvec_1, [tvec2], ...]] position of the marker in camera frame
ret = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion)
#-- Unpack the output, get only the first\n",
rvec, tvec = ret[0][0,0,:], ret[1][0,0,:]
#-- Draw the detected marker and put a reference frame over it\n",
aruco.drawDetectedMarkers(src_image, corners)
aruco.drawAxis(src_image, camera_matrix, camera_distortion, rvec, tvec, 0.3)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T # function transpose() with '.T'
#-- Get the attitude in terms of euler 321 (Needs to be flipped first)
pitch_marker, roll_marker, yaw_marker = rotationMatrixToEulerAngles(R_tc)
#-- Now get Position and attitude f the camera respect to the marker
#pos_camera = -R_tc*np.matrix(tvec).T
pitch_camera, roll_camera, yaw_camera = rotationMatrixToEulerAngles(R_flip*R_tc)
pos_camera = Point(-tvec[0], tvec[1], tvec[2])
#-- Print 'X' in the center of the camera
cv2.putText(src_image, "X", (cols/2, rows/2), font, 1, (0, 0, 255), 2, cv2.LINE_AA)
###############################################################################
#-- Print the tag position in camera frame
str_position = "Position x = %4.0f y = %4.0f z = %4.0f"%(pos_camera.x, pos_camera.y, pos_camera.z)
cv2.putText(src_image, str_position, (0, 30), font, 2, (255, 255, 0), 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
str_attitude = "Attitude pitch = %4.0f roll = %4.0f yaw = %4.0f"%(math.degrees(0),math.degrees(0),
math.degrees(yaw_camera))
cv2.putText(src_image, str_attitude, (0, 60), font, 2, (255, 255, 0), 2, cv2.LINE_AA)
###############################################################################
#rospy.loginfo('Id detected!')
#else:
#rospy.loginfo('No Id detected!')
#print('No Id detected!')
foco = 527
x = (foco*self.vec.x)/self.vec.z
y = (foco*self.vec.y)/self.vec.z
X = x+(cols/2)
Y = y+(rows/2)
print(X)
# Center coordinates
center_coordinates = (int(X), int(Y))
#center_coordinates = (cols/2, rows/2)
# Radius of circle
radius = 20
# Blue color in BGR
color = (255, 0, 0)
# Line thickness of 3 px
thickness = 3
# Using cv2.circle() method
# Draw a circle with blue line borders of thickness of 2 px
src_image = cv2.circle(src_image, center_coordinates, radius, color, thickness)
cv2.imshow("Image", src_image)
cv2.waitKey(1)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(src_image, "bgr8"))
except CvBridgeError as e:
print(e)
def detect_aruco(self, frame):
# dictionary - 4x4 aruco images
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
# aruco_dict = aruco.generateCustomDictionary(43,4)
# print(aruco_dict)
# get gray picture
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# gray = frame[:,:,0].astype(np.uint8)
# get aruco frames
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# print(ids)
# put aruco detected markers on top of colored frame (webcam input)
# frame_markers = aruco.drawDetectedMarkers(gray.copy(), corners, ids)
frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
# get (x,y) center coordinates of aruco marker
# if only one marker is present
if ids is not None:
# get number of matched IDs
count_matches = 0
# get number of aruco tags
for i in range(len(ids)):
if ids[i][0] == NK_ARUCO_ID:
count_matches = count_matches + 1
match_idx = i
# move cursor only if exactly one valid ArUco symbol is detected
if count_matches == 1:
c = corners[match_idx][0]
# print(c)
# detect one point only
# x = c[0][0]
# y = c[0][1]
x = c[:, 0].mean()
y = c[:, 1].mean()
# draw center point
draw_x1 = int(round(x)) - 5
draw_y1 = int(round(y)) - 5
draw_x2 = int(round(x)) + 5
draw_y2 = int(round(y)) + 5
frame_markers = cv2.rectangle(frame_markers,
(draw_x1, draw_y1),
(draw_x2, draw_y2), (0, 255, 0),
3)
# print("c[" + str(i) + "] = " + str(c[i]))
# print("x = " + str(x))
# print("y = " + str(y))
# print("")
# move cursor if checkbox is checked
move = 0
if self.moveCursorCheckBox.checkState():
move = 1
# move cursor if needed
self.move_cursor(move, x, y)
return frame_markers
parameters = aruco.DetectorParameters_create()
x = 8 # horizontal
y = 11 # vertical
sqr = 0.1 # solid black squares
mrk = 0.05 # markers, must be smaller than squares
board = aruco.CharucoBoard_create(x,y,sqr,mrk,dictionary)
imgs = get_images("cal-imgs/*.png", gray=False)
calcorners = []
calids = []
for im in imgs:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
# print('ids 1:', len(ids))
# if ids were found, then
if len(ids) > 0:
# print(corners, ids, rejectedImgPoints)
ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
calcorners.append(chcorners)
calids.append(chids)
im2 = im.copy()
aruco.drawDetectedCornersCharuco(im2, chcorners, chids)
# aruco.drawDetectedMarkers(im2, rejectedImgPoints, borderColor=(100, 0, 240))
cv2.imshow('markers', im2)
cv2.waitKey()
def scanSubprocess(self, timestamp, timeout, image, offsetx, offsety, resultsque):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
self.corners, self.ids, self.rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters)
resultsque.put([timestamp, self.corners, self.ids, self.rejectedImgPoints])
resultsque.close()
size = args["marker_with_border"] / 2
nsize = -1 * size
axis = np.float32([[nsize, size, 0], [size, size, 0], [size, nsize, 0], [nsize, nsize, 0]])
counter = 0
# Now comes the recording part
print("Press [SPACE] to record a sample and [ESC] to close the application:")
while(True):
# Get frame from realsense, convert to bgr and create a grayscale image for marker detection
frames = pipeline.wait_for_frames()
frame = cv2.cvtColor(np.asanyarray(frames.get_color_frame().get_data()), cv2.COLOR_RGB2BGR)
display_frame = deepcopy(frame)
frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect markers and draw them
corners, ids, rejectedImgPoints = aruco.detectMarkers(frame_gray, aruco.getPredefinedDictionary(dictionary))
if ids is not None and len(ids) > 0:
display_frame = aruco.drawDetectedMarkers(display_frame, corners, ids)
# Create the gt mask
mask = np.zeros(shape=(display_frame.shape[0], display_frame.shape[1], display_frame.shape[2])).astype(np.uint8)
if ids is not None and len(ids) > 0:
for marker in corners:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(marker, 1, mtx, dist)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
imgpts = np.int32(imgpts).reshape(-1, 2)
mask = cv2.drawContours(mask, [imgpts[:4]], -1, (255, 255, 255), -1)
# Display the current frame with the corresponding mask
cv2.imshow("Record", np.hstack((cv2.resize(display_frame, (0,0), fx=0.5, fy=0.5) , cv2.resize(mask, (0,0), fx=0.5, fy=0.5))))
key = cv2.waitKey(100)
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 precess(self):
isfinish = False
rgb = cv2.cvtColor(self.arr, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(self.arr, cv2.COLOR_BGR2GRAY)
dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
corners, ids, rejectedPoints = aruco.detectMarkers(
gray, dictionary, parameters=aruco.DetectorParameters_create())
### draw marker
aruco.drawDetectedMarkers(rgb, corners, ids)
if len(corners) == 4:
self.isDetect4Marker = True
for i in ids:
if i[0] not in [0, 1, 2, 3]:
self.isDetect4Marker = False
self.corners = corners
self.ids = ids
if self.isDetect4Marker:
plotMarker(rgb, self.corners, self.ids)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
self.corners, self.arucoSize, self.mtx, self.dist)
Rpose = from_cam_to_robot(tvecs,
-np.pi / 6) ### pose for robot coor
PoseID = self.ids
for i in range(rvecs.shape[0]):
aruco.drawAxis(rgb, self.mtx, self.dist, rvecs[i, :, :],
tvecs[i, :, :], 0.05)
### find Internal Corners
insidecorners = findInternalCorner(self.corners)
### draw boundary
cv2.polylines(rgb, [np.array(insidecorners, dtype=int)], True,
(0, 255, 0), 4)
Pdst = np.array(
[[0, 0], [self.w, 0], [self.w, self.h], [0, self.h]],
np.float32)
### detect circle
hsv = cv2.cvtColor(self.arr, cv2.COLOR_BGR2HSV)
lower = np.array([0, 0, 0], np.uint8)
upper = np.array([180, 255, 50], np.uint8)
sep = cv2.inRange(hsv, lower, upper)
kernel = np.ones((9, 9), np.float32) / 81
sep = cv2.filter2D(sep, -1, kernel)
M = cv2.getPerspectiveTransform(insidecorners, Pdst)
warped = cv2.warpPerspective(sep, M, (self.w, self.h))
warped = 255 - warped
circles = cv2.HoughCircles(warped,
cv2.HOUGH_GRADIENT,
1,
10,
param1=1,
param2=30,
minRadius=30,
maxRadius=50)
if circles is not None:
### remove circle detect the aruco
new_circles = []
for i in circles[0]:
if not (i[1] > self.h - self.aS and i[0] < self.aS):
if not (i[1] > self.h - self.aS
and i[0] > self.w - self.aS):
new_circles.append(i)
cv2.circle(warped, (i[0], i[1]), i[2], (0, 0, 255),
2)
circles = np.array(new_circles)
cv2.imshow('gray', warped)
cv2.waitKey(1)
if len(circles) >= 3:
kmeans = KMeans(n_clusters=3, random_state=0).fit(circles)
c = kmeans.labels_
center = kmeans.cluster_centers_
dis0 = np.sqrt(
(center[0] - center[1]).dot(center[0] - center[1]))
dis1 = np.sqrt(
(center[1] - center[2]).dot(center[1] - center[2]))
dis2 = np.sqrt(
(center[2] - center[0]).dot(center[2] - center[0]))
### check detect three tube in the work space
if dis0 > 50 and dis1 > 50 and dis2 > 50:
print("three tube")
index = np.argsort(
[center[0][0], center[1][0], center[2][0]])
circles = np.uint16(np.around(circles))
maxCA = 0
maxhA = 0
maxCB = 0
maxhB = 0
maxCC = 0
maxhC = 0
for i in range(len(circles)):
## left tube
if c[i] == index[0]:
if circles[i][1] > maxhA:
maxCA = circles[i]
## mid tube
if c[i] == index[1]:
if circles[i][1] > maxhB:
maxCB = circles[i]
## right tube
if c[i] == index[2]:
if circles[i][1] > maxhC:
maxCC = circles[i]
self.circleList[0].append(maxCA)
self.circleList[1].append(maxCB)
self.circleList[2].append(maxCC)
L = len(self.circleList[0])
if L > 50:
warped = cv2.cvtColor(warped, cv2.COLOR_GRAY2RGB)
p = Position()
tube = np.sum(self.circleList[0][L - 10:L - 1],
axis=0) / 9
cv2.circle(warped, (tube[0], tube[1]), tube[2],
(0, 0, 255), 2)
cv2.circle(warped, (tube[0], tube[1]), 2,
(0, 0, 255), 3)
### translate to Robot coord
IDs = IDAruco2IDCorner(PoseID, 0)
tubeX = Rpose[IDs][0] - (
tube[1] * self.workSpaceW /
self.w) - self.arucoSize / 2 + 0.02
tubeY = Rpose[IDs][1] - (
tube[0] * self.workSpaceH /
self.h) + self.arucoSize / 2
tubeZ = Rpose[IDs][2]
p.Ax = tubeX
p.Ay = tubeY
p.Az = tubeZ
p.A = True
tube = np.sum(self.circleList[1][L - 10:L - 1],
axis=0) / 9
cv2.circle(warped, (tube[0], tube[1]), tube[2],
(0, 0, 255), 2)
cv2.circle(warped, (tube[0], tube[1]), 2,
(0, 0, 255), 3)
### translate to Robot coord
IDs = IDAruco2IDCorner(PoseID, 0)
tubeX = Rpose[IDs][0] - (
tube[1] * self.workSpaceW /
self.w) - self.arucoSize / 2 + 0.02
tubeY = Rpose[IDs][1] - (
tube[0] * self.workSpaceH /
self.h) + self.arucoSize / 2
tubeZ = Rpose[IDs][2]
p.Bx = tubeX
p.By = tubeY
p.Bz = tubeZ
p.B = True
tube = np.sum(self.circleList[2][L - 10:L - 1],
axis=0) / 9
cv2.circle(warped, (tube[0], tube[1]), tube[2],
(0, 0, 255), 2)
cv2.circle(warped, (tube[0], tube[1]), 2,
(0, 0, 255), 3)
### translate to Robot coord
IDs = IDAruco2IDCorner(PoseID, 0)
tubeX = Rpose[IDs][0] - (
tube[1] * self.workSpaceW /
self.w) - self.arucoSize / 2 + 0.02
tubeY = Rpose[IDs][1] - (
tube[0] * self.workSpaceH /
self.h) + self.arucoSize / 2 - 0.02
tubeZ = Rpose[IDs][2]
p.Cx = tubeX
p.Cy = tubeY
p.Cz = tubeZ
p.C = True
IDs = IDAruco2IDCorner(PoseID, 3)
tubeX = Rpose[IDs][0]
tubeY = Rpose[IDs][1]
tubeZ = Rpose[IDs][2]
IDs = IDAruco2IDCorner(PoseID, 2)
tubeX1 = Rpose[IDs][0]
tubeY1 = Rpose[IDs][1]
tubeZ1 = Rpose[IDs][2]
p.Dx = (tubeX + tubeX1) / 2
p.Dy = (tubeY + tubeY1) / 2
p.Dz = (tubeZ + tubeZ1) / 2
p.D = True
cv2.imshow('gray', warped)
cv2.waitKey(1)
if raw_input("tube position is correct?") == "yes":
self.pubTubePosition.publish(p)
#print(os.path.dirname(__file__))
isfinish = True
#cv2.imwrite(os.path.dirname(__file__) + "/gray.png",warped)
#cv2.imwrite(os.path.dirname(__file__) + "/img.png",rgb)
cv2.imshow('img', rgb)
cv2.waitKey(1)
return isfinish
#start combination cal.
for a in range(0,20):
print(a)
for j in range(0,20):
for k in range(0,20):
for d in range(0,20):
#첫번째 20단위로 실행시
distCoefss = (-0.060285+a*0.0054385, -0.500998+j*0.03915, -0.003908+k*0.000336,-0.004445+d*0.0003888)
#두번째
#distCoeffs =(-0.0396187 + (a*0.0010877), -0.195628+ (j*0.00783), -0.0035172+ (k * 0.000672), -0.00063476+(d * 0.0000777))
while(True):
ret, image = cap.read()
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(image=image, dictionary=arucoDict, cameraMatrix=cameraMatrix, distCoeff=distCoeffs, parameters = parameters)
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, 0.06, cameraMatrix, distCoeffs)
if ids.size == 9:
temp = [(),(),(),(),(),(),(),(),()]
for i in range(0,ids.size):
idr = ids[i][0]
#x = (corners[i][0][0][0] + corners[i][0][1][0] + corners[i][0][2][0] + corners[i][0][3][0]) / 4
#y = (corners[i][0][0][1] + corners[i][0][1][1] + corners[i][0][2][1] + corners[i][0][3][1]) / 4
def processFrame(self): # grab the frame from the threaded video stream while(True): frame, repeated = self.vs.read() if(not repeated): break # detect aruco markers in the frame gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, idsM, rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters, cameraMatrix=self.refinedMatrix, distCoeff=self.distCoeffs) #remove detected markers that aren't defined in marker_points.txt, and change format to array from matrix ids = [] if(idsM is not None): #change ids format to array for i in idsM: ids = np.append(ids, i[0]) #remove undefined ids for i in ids: if(i not in self.markerIds): pos = np.where(ids == i)[0][0] #the reason for these brackets is to undo numpy's return of array of arrays ids = np.delete(ids, pos) success = False world_coordinates = [] rot_vector = [] trans_vector = [] retIds = [] #if a defined marker has been detected if(len(ids) > 0): #create objPoints depending on which markers were detected, respecting #the order of 'corners' and 'ids' objPoints = np.empty((0,3),float) imgPoints = np.empty((0,2), float) for i in ids: #self.markerIds may be unordered, so we must find the index of each id pos = np.where(self.markerIds == i)[0][0] #extract the corresponding points of the id markerIPoints = self.markerPoints[pos*4:(pos+1)*4][:] #insert the matrix into our final objPoints objPoints = np.vstack((objPoints, markerIPoints)) #insert the corresponding detected corners into imgPoints posCorner = np.where(idsM == i)[0][0] imgPoints = np.vstack((imgPoints, corners[posCorner][0])) retIds.append(i) #show the frame, with detected markers gray = aruco.drawDetectedMarkers(gray, corners) success, rotation_vector, translation_vector = cv2.solvePnP(objPoints, imgPoints, self.refinedMatrix, self.distCoeffs, flags=cv2.SOLVEPNP_ITERATIVE) translation_vector.mean() if(success): #solvePnP's x-axis rotation angle is of opposite sign relative to the Y coordinate if(self.applyDisplacement): for i in range(3): translation_vector[i] = translation_vector[i] + self.displacementArray[i] world_coordinates = self.__camera_to_world_coords(rotation_vector, translation_vector) rotation_vector[0] = -rotation_vector[0] if(self.inverseX): world_coordinates[0] = -world_coordinates[0] if(self.inverseY): world_coordinates[1] = -world_coordinates[1] if(self.inverseZ): world_coordinates[2] = -world_coordinates[2] #turn rotation_vector from radians to degrees rotation_vector = rotation_vector / math.pi * 180 if(self.inverseXAngle): rotation_vector[0] = -rotation_vector[0] if(self.inverseYAngle): rotation_vector[1] = -rotation_vector[1] if(self.inverseZAngle): rotation_vector[2] = -rotation_vector[2] #build output arrays for i in range(3): rot_vector.append((rotation_vector[i]).item()) trans_vector.append((translation_vector[i]).item()) return success, world_coordinates, rot_vector, trans_vector, retIds, gray
cv2.imshow("marker", aruco.drawMarker(adict, 0, 400))
# random calibration data. your mileage may vary.
imsize = (800, 600)
K = cv2.getDefaultNewCameraMatrix(np.diag([800, 800, 1]), imsize, True)
# AR scene
cv2.ovis.addResourceLocation("packs/Sinbad.zip") # shipped with Ogre
win = cv2.ovis.createWindow("arucoAR", imsize, flags=0)
win.setCameraIntrinsics(K, imsize)
win.createEntity("figure", "Sinbad.mesh", (0, 0, 5), (1.57, 0, 0))
win.createLightEntity("sun", (0, 0, 100))
# video capture
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, imsize[0])
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, imsize[1])
while cv2.ovis.waitKey(1) != 27:
img = cap.read()[1]
win.setBackground(img)
corners, ids = aruco.detectMarkers(img, adict)[:2]
cv2.waitKey(1)
if ids is None:
continue
rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, 5, K, None)[:2]
win.setCameraPose(tvecs[0].ravel(), rvecs[0].ravel(), invert=True)
img = np.full((700, 700), 255, np.uint8)
# Pega primer marcador (Imagen de 200x200)
img[100:300, 100:300] = aruco.drawMarker(aruco_dict, 2, 200)
# Pega segundo marcador (Imagen de 200x200)
img[100:300, 400:600] = aruco.drawMarker(aruco_dict, 76, 200)
# Pega tercer marcador (Imagen de 200x200)
img[400:600, 100:300] = aruco.drawMarker(aruco_dict, 42, 200)
# Pega cuarto marcador (Imagen de 200x200)
img[400:600, 400:600] = aruco.drawMarker(aruco_dict, 123, 200)
# Difuminado de imagen
img = cv2.GaussianBlur(img, (11, 11), 0)
# Localización de los marcadores
corners, ids, _ = aruco.detectMarkers(img, aruco_dict)
# Obtiene uan versión en escala de grises de los marcadores
img_color = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
# Dibujar los marcadores
aruco.drawDetectedMarkers(img_color, corners, ids)
# Muestra Imagenes
cv2.imshow('Created AruCo markers', img)
cv2.imshow('Detected AruCo markers', img_color)
cv2.waitKey(0)
cv2.destroyAllWindows()
cap = cv2.VideoCapture(0)
while True:
def track(matrix_coefficients, distortion_coefficients, markerLength,
frame_rate):
print('check markerLengt', markerLength)
global cX_ini
global cY_ini
global cY
global cX
global data_blow
global data_value_dis
global data_check
global tvec_ini
global tvec
global blow
global dist
global data_tvecs
global value_dis
global value_blow
global pile_drive
global i
data_tvecs = [] # Prepare to add tvec
value_dis = [] # Prepare to get result distance(scalar) unit meter
value_blow = []
sum_tvec = 0
tvec_ini = np.array([0, 0, 0])
tvec = np.array([0, 0, 0])
dist = 0
prev = 0
blow = 0.00
cX = 0
cY = 0
cX_ini = 300
cY_ini = 50
check = []
while cap.isOpened():
#convert image to gray scale image
#Capture frame-by-frame
time_elapsed = time.time() - prev
ret, frame = cap.read()
#operations on the frame come here
if not ret:
continue
if time_elapsed > 1. / frame_rate:
prev = time.time()
#frame = cv2.resize(frame, (4000, 5000))
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Change grayscale
aruco_dict = aruco.Dictionary_get(
aruco.DICT_5X5_250) # Use 5x5 dictionary to find markers
parameters = aruco.DetectorParameters_create(
) # Marker detection parameters
# lists of ids and the corners beloning to each id
corners, ids, rejected_img_points = aruco.detectMarkers(
gray,
aruco_dict,
parameters=parameters,
cameraMatrix=np.float32(matrix_coefficients),
distCoeff=np.float32(distortion_coefficients))
# calculate(ids,corners,matrix_coefficients,distortion_coefficients,frame)
if np.all(
ids is not None): # If there are markers found by detector
for i in range(0, len(ids)): # Iterate in markers
# Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(
corners[i], markerLength,
np.float32(matrix_coefficients),
np.float32(distortion_coefficients))
(rvec - tvec
).any() # get rid of that nasty numpy value array error
aruco.drawDetectedMarkers(
frame, corners) # Draw A square around the markers
#aruco.drawAxis(frame, np.float32(matrix_coefficients), np.float32(distortion_coefficients),np.float32(rvec), np.float32(tvec), 0.01) # Draw Axis
data_tvecs.append(
tvec) # get every data of tvec in array data_tvecs
length = len(data_tvecs) # For easy to calculate equation
if length > 1:
diff = data_tvecs[
length -
1] - tvec_ini # Find distance between tvec(vector)
sum_tvec = sqrt(np.sum(
diff**2)) # convert vector to scalar
dist = sum_tvec
value_dis.append(
np.round(sum_tvec, 3)
) # get the distance data(scalar) in array value
#calculate move per blow
length = len(value_dis)
if length > 10:
pile_drive = []
for x in range(10):
pile_drive.append(value_dis[length - x - 1])
#print('pile drive before', pile_drive)
if np.std(pile_drive) <= .005:
#print('pile drive after', pile_drive)
blow = float(np.average(pile_drive))
value_blow.append(np.round(blow, 4))
check = pd.Series(value_blow)
check = check.unique()
value_dis = []
pile_drive = []
if len(corners) > 0:
# flatten the ArUco IDs list
ids = ids.flatten()
# loop over the detected ArUCo corners
for (markerCorner, markerID) in zip(corners, ids):
# extract the marker corners (which are always returned in
# top-left, top-right, bottom-right, and bottom-left order)
find_center = corners.copy()
find_center = markerCorner.reshape((4, 2))
(topLeft, topRight, bottomRight, bottomLeft) = find_center
# convert each of the (x, y)-coordinate pairs to integers
topRight = (int(topRight[0]), int(topRight[1]))
bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
topLeft = (int(topLeft[0]), int(topLeft[1]))
# draw the bounding box of the ArUCo detection
#cv2.line(frame, topLeft, topRight, (0, 255, 0), 2)
#cv2.line(frame, topRight, bottomRight, (0, 255, 0), 2)
#cv2.line(frame, bottomRight, bottomLeft, (0, 255, 0), 2)
#cv2.line(frame, bottomLeft, topLeft, (0, 255, 0), 2)
# compute and draw the center (x, y)-coordinates of the ArUco
# marker
cX = int((topLeft[0] + bottomRight[0]) / 2.0)
cY = int((topLeft[1] + bottomRight[1]) / 2.0)
cv2.circle(frame, (cX, cY), 4, (0, 0, 255), -1)
# draw the ArUco marker ID on the image
cv2.putText(frame, str(markerID),
(topLeft[0], topLeft[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.putText(frame, str(topRight),
(topRight[0], topRight[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.circle(frame, (cX_ini, cY_ini), 4, (0, 255, 0), -1)
fps = 1 / time_elapsed
fps = np.round(fps, 1)
cv2.putText(frame, 'F5PS ' + str(fps), (50, 75),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
data_value_dis = pd.DataFrame(value_dis, columns=['real Distance'])
#data_blow = pd.DataFrame(value_blow, columns = ['avg blow'])
#data_check = pd.DataFrame(check, columns = ['check'])
#for gui
image_in = frame
image_in = Image.fromarray(image_in)
vdo.image = image_in
diff_text = "{:.3f}".format(float(dist))
blow_show = "{:.3f}".format(float(blow))
label1.text = 'distance= ' + diff_text + ' m'
#label2.text ='deform per blows='+blow_show + ' m'
#label3.text ='deform per blows realtime ='+ str(np.round(blow,4))+ ' m'
return
camera = PiCamera()
#camera.resolution = (1040, 784)
camera.resolution = (1024, 770)
camera.framerate = 30
rawCapture = PiRGBArray(camera, size=(1024, 770))
time.sleep(0.1)
tvec0 = np.array([[[0.0, 0.0, 0.0]]])
rvecmax = 0.0
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
image = frame.array
cv2.rectangle(image, (0, 0), (200, 200), (220, 240, 230), -1)
corners, ids, rejectedImgPoints = aruco.detectMarkers(image, aruco_dict)
image = aruco.drawDetectedMarkers(image, corners) # marker körvonalak
rvec, tvec ,_ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, distor)
if ids is not None:
for i in range(0, ids.size):
#print(image.dtype)
rr, thet = ra.rArea(corners)
aruco.drawAxis(image, mtx, distor, rvec[0], tvec[0], 0.06) # np.array([0.0, 0.0, 0.0])
cv2.putText(image, "%.1f cm -- %.0f deg" % ((tvec[0][0][2] * 100), (rvec[0][0][2] / math.pi * 180)), (0, 230), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (244, 244, 244))
cv2.circle(image, (100, int(rr / 600)), 6, (200, 40, 230), -1)
R, _ = cv2.Rodrigues(rvec[0])
cameraPose = -R.T * tvec[0]
cv2.imshow("Frame", image)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
def image_cb(self, msg):
# get image from camera
image = self.bridge.imgmsg_to_cv2(msg)
# transfer to grey image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# define the dictionary
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
# define parameters for aruco_detect
parameters = aruco.DetectorParameters_create()
# calibration matrices
# default matrices used here since there is no distortion in simulation
camera_matrix = np.array(
[[image.shape[1], 0, image.shape[1]/2],
[0, image.shape[1], image.shape[0]/2],
[0, 0, 1]], dtype = "double"
)
dist = np.array( [0, 0, 0, 0, 0] )
# get ID and corners
corners, ids, rejected_img_points = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# when there is marker in sight
if ids is not None:
# get image dimention
h, w, d = image.shape
# update id that is seen
self.id = max(ids)
# estimate pose of each marker
ret = aruco.estimatePoseSingleMarkers(corners, 0.2, camera_matrix, dist)
rvec, self.tvec = ret[0][0,0,:], ret[1][0,0,:]
# clear numbers
(rvec - self.tvec).any()
# draw contours of markers
aruco.drawDetectedMarkers(image, corners)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(image, "Id: " + str(self.id), (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)
# find the center pixel of the marker with largest id
x_sum = corners[0][0][0][0]+corners[0][0][1][0]+corners[0][0][2][0]+corners[0][0][3][0]
y_sum = corners[0][0][0][1]+corners[0][0][1][1]+corners[0][0][2][1]+corners[0][0][3][1]
cx = int(x_sum*.25)
cy = int(y_sum*.25)
# put a circle there
cv2.circle(image, (cx, cy), 15, (0,0,255), -1)
# use "P" control to move
err = cx - w/2
self.twist.linear.x = 0.1
self.twist.angular.z = -float(err) / 500 / 4
self.cmd_vel_pub.publish(self.twist)
ids = None
# when there's no marker in sight
else:
if min(self.ranges[0:30]) < 1.2 or min(self.ranges[330:360]) < 1.2:
if min(self.ranges[0:100]) < min(self.ranges[260:360]):
# turn right
self.twist.angular.z = ABS_ROTATE * (-1)
self.twist.linear.x = FORWARD_SPEED
else:
# turn left
self.twist.angular.z = ABS_ROTATE
self.twist.linear.x = FORWARD_SPEED
self.cmd_vel_pub.publish(self.twist)
else:
pid = self.calc_pid()
self.twist.angular.z = ABS_ROTATE * pid
self.twist.linear.x = FORWARD_SPEED
self.cmd_vel_pub.publish(self.twist)
# show the image window
cv2.imshow('image', image)
cv2.waitKey(3)
marker_length = .20955 #in meters
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
#load intrinsic
camera_mtx = np.asmatrix(np.loadtxt("cameramatrix.txt"))
camera_dist = np.loadtxt("cameradistortion.txt")
img = cv2.imread("aruco.jpg")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
parameters = aruco.DetectorParameters_create()
#find markers
corners, idx, rejected = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
print(corners)
img = aruco.drawDetectedMarkers(img, corners)
for rect in rejected:
print("Rect:" + str(rect))
for point in rect[0]:
cv2.circle(img, (point[0], point[1]), 2, (0, 255, 0), -1)
#get pose
rvecs, tvecs, obj_points = aruco.estimatePoseSingleMarkers(
corners, marker_length, camera_mtx, camera_dist)
#save rotation and translation
np.savetxt("rotationvector.txt", rvecs[0])
while cap.isOpened():
fn += 1
im_src = cv2.imread('../media/icon2.png')
# Capture frame-by-frame
ret, frame = cap.read()
org_frame = frame
# Check if frame is not empty
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, _ = aruco.detectMarkers(frame,
aruco_dict,
parameters=parameters)
if np.all(ids is not None):
# print(np.array(corners))
# print(corners)
print(ids)
for c in corners:
x1 = (c[0][0][0], c[0][0][1])
x2 = (c[0][1][0], c[0][1][1])
x3 = (c[0][2][0], c[0][2][1])
x4 = (c[0][3][0], c[0][3][1])
im_dst = frame
size = im_src.shape
pts_dst = np.array([x1, x2, x3, x4])
d = np.linalg.norm(a - b)
while (1):
ret, frame = cam.read()
ret, frame2 = cam.read()
ret, frame3 = cam.read()
ret, frame5 = cam.read()
if not ret:
break
frame4 = cv2.cvtColor(frame3, cv2.COLOR_BGR2GRAY)
cv2.rectangle(frame4, (10, 10), (100, 100), color=(255, 0, 0), thickness=3)
corners, ids, rejected = aruco.detectMarkers(frame4,
aruco_dict,
parameters=parameters)
canvas = frame.copy()
canvas2 = frame2.copy()
# lower = (220,80,80) #130,150,80
# upper = (240,100,100) #250,250,120
# Black
# lower = np.array([110, 80, 20])
# upper = np.array([140, 255, 255])
# Green
Grlower = np.array([0, 143, 0])
Grupper = np.array([102, 255, 255])
def scan(self):
# Scan each zone.
for z in self.zones:
z.getImage()
timestamp = time.time()
# If the zone is calibrated
if z.calibrated:
# Warp the zone part of the image and make it rectangular.
z.warpImage()
# Handle any returned symbol data.
# while self.procman.resultsAvailable():
# data = self.procman.getResult()
# if len(data) == 2:
# ts = data[0] # timestamp
# symbols = data[1]
# else:
# continue
#
# # Remove finished processes from the pool.
# self.procman.removeFinished()
#
# # Scan for all known markers.
# #for marker in self.markers.iteritems():
# #roi = z.image[c.roiminy:c.roimaxy, c.roiminx:c.roimaxx]
# #self.procman.addProcess(timestamp, self.scantimeout, roi, c.roiminx, c.roiminy)
#
# # Blank the region of the image where the symbol was last seen.
# # Remove jitter by no includeing symbol[2]
# #s = copy(c.symbol)
# #s.pop(2)
# #s = np.delete()
# #print c.symbol
# #print s,"\n"
#
# #poly = array(c.symbol, int32)
# #cv2.fillConvexPoly(z.image, poly, (255,255,255))
# #cv2.putText(z.image, str(c.id), (c.location[0]-8, c.location[1]+8), cv2.FONT_HERSHEY_PLAIN, 1.5, c.color_augtext, 2)
#
# # Scan for a new symbol.
# self.procman.addProcess(timestamp, self.scantimeout, z.image)
# If the zone is not calibrated
else:
#Convert image to grayscale and detect markers.
gray = cv2.cvtColor(z.image, cv2.COLOR_BGR2GRAY)
self.corners, self.ids, self.rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters)
if self.ids is not None:
for idx,foundid in enumerate(self.ids):
markerid = foundid[0] # I have no idea why the ids are a 2 dimensional list.
if markerid not in self.markers:
self.markers[markerid] = self.corners[idx][0]
self.markerfoundcount[markerid] = 1
else:
if self.markerfoundcount[markerid] < self.calibrationMin:
self.markerfoundcount[markerid] += 1
# Store the calibrationMin of corners and ids
if len(self.corners) > 0:
retval, charucoCorners, charucoIds = cv2.aruco.interpolateCornersCharuco(self.corners, self.ids, gray, z.projector.board)
if charucoCorners is not None and charucoIds is not None and len(charucoCorners)==len(charucoIds):
self.calibrationCorners.append(charucoCorners)
self.calibrationIds.append(charucoIds)
if len(self.calibrationCorners) > self.calibrationMin:
# remove the oldest
self.calibrationCorners.pop(0)
self.calibrationIds.pop(0)
# Ready to calibrate when all markers have been found the minimum number of times.
ready = True
if len(self.markers)-1 == z.projector.maxcalmarkerid:
for idx in range(0,z.projector.maxcalmarkerid+1):
if self.markerfoundcount[idx] < self.calibrationMin:
ready = False
break
else:
ready = False
if ready:
#Capture frames to use the in camera calibration
print "Calibrating..."
try:
retval, z.cameraMatrix, z.distCoefs, z.rvecs, z.tvecs = cv2.aruco.calibrateCameraCharuco(self.calibrationCorners, self.calibrationIds, z.projector.board, gray.shape,None,None)
#print(retval, cameraMatrix, distCoeffs, rvecs, tvecs)
print "Calibration successful"
except:
print "Calibration failed"
z.calibrated = ready
#End of zone loop
return
def roda_todo_frame(imagem):
#print("frame")
try:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem,
"bgr8") # imagem compressed
#cv_image = cv2.flip(cv_image, -1) # Descomente se for robo real
#cv_image = bridge.imgmsg_to_cv2(imagem, "bgr8") # imagem não compressed
#cv_image = cv2.resize(cv_image,(cv_image.shape[1]*2,cv_image.shape[0]*2)) # resize image se necessario
gray = cv2.cvtColor(cv_image, 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(cv_image, corners, ids)
aruco.drawAxis(cv_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(cv_image, str_position, (0, 100), font, 1, (0, 255, 0),
1, cv2.LINE_AA)
##############----- Referencia dos Eixos------###########################
# Linha referencia em X
cv2.line(
cv_image,
(int(cv_image.shape[1] / 2), int(cv_image.shape[0] / 2)),
(int(cv_image.shape[1] / 2 + 50), int(cv_image.shape[0] / 2)),
(0, 0, 255), 5)
# Linha referencia em Y
cv2.line(
cv_image,
(int(cv_image.shape[1] / 2), int(cv_image.shape[0] / 2)),
(int(cv_image.shape[1] / 2), int(cv_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(cv_image, str_dist, (0, 15), font, 1, (0, 255, 0), 1,
cv2.LINE_AA)
#####################---- Distancia pelo foco ----#####################
#https://www.pyimagesearch.com/2015/01/19/find-distance-camera-objectmarker-using-python-opencv/
# raspicam v2 focal legth
FOCAL_LENGTH = 3.6 #3.04
# pixel por unidade de medida
m = (camera_matrix[0][0] / FOCAL_LENGTH +
camera_matrix[1][1] / FOCAL_LENGTH) / 2
# corners[0][0][0][0] = [ID][plano?][pos_corner(sentido horario)][0=valor_pos_x, 1=valor_pos_y]
pixel_length1 = math.sqrt(
math.pow(corners[0][0][0][0] - corners[0][0][1][0], 2) +
math.pow(corners[0][0][0][1] - corners[0][0][1][1], 2))
pixel_length2 = math.sqrt(
math.pow(corners[0][0][2][0] - corners[0][0][3][0], 2) +
math.pow(corners[0][0][2][1] - corners[0][0][3][1], 2))
pixlength = (pixel_length1 + pixel_length2) / 2
dist = marker_size * FOCAL_LENGTH / (pixlength / m)
#-- Print distancia focal
str_distfocal = "Dist focal=%4.0f" % (dist)
print(str_distfocal)
cv2.putText(cv_image, str_distfocal, (0, 30), font, 1, (0, 255, 0),
1, cv2.LINE_AA)
####################--------- desenha o cubo -----------#########################
# https://github.com/RaviJoshii/3DModeler/blob/eb7ca48fa06ca85fcf5c5ec9dc4b562ce9a22a76/opencv/program/detect.py
m = marker_size / 2
pts = np.float32([[-m, m, m], [-m, -m, m], [m, -m, m], [m, m, m],
[-m, m, 0], [-m, -m, 0], [m, -m, 0], [m, m, 0]])
imgpts, _ = cv2.projectPoints(pts, rvec, tvec, camera_matrix,
camera_distortion)
imgpts = np.int32(imgpts).reshape(-1, 2)
cv_image = cv2.drawContours(cv_image, [imgpts[:4]], -1,
(0, 0, 255), 4)
for i, j in zip(range(4), range(4, 8)):
cv_image = cv2.line(cv_image, tuple(imgpts[i]),
tuple(imgpts[j]), (0, 0, 255), 4)
cv_image = cv2.drawContours(cv_image, [imgpts[4:]], -1,
(0, 0, 255), 4)
# Exibe tela
cv2.imshow("Camera", cv_image)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
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
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)
def ar_marker_pose_estimation(req):
print("Start AR Marker Pose Estimation")
ar_pub = rospy.Publisher('/AR/Estimation_image', Image, queue_size=10)
pose_pub = rospy.Publisher('/AR/Estimation_pose', PoseArray, queue_size=10)
try:
gray = cv2.cvtColor(cv_image, 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(cv_image.copy(), corners,
ids)
markerLength = 0.07
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL +
cv2.CALIB_FIX_ASPECT_RATIO)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, markerLength, cameramatrix, dist_coeffs)
total_rvec = np.zeros([60, 1, 3])
total_tvec = np.zeros([60, 1, 3])
length_of_axis = 0.05
total_center = np.zeros(shape=[60, 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
total_rvec[ids[index] - 1] = rvecs[index]
total_tvec[ids[index] - 1] = tvecs[index]
imaxis = aruco.drawDetectedMarkers(cv_image.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
ar_pub.publish(bridge.cv2_to_imgmsg(imaxis, 'bgr8'))
## get only 1 point data from point clouds
## uvs must be iterable, so use 2d list.
pose_arr = PoseArray()
index_ids = ids.reshape(ids.shape[0])
for point_i in index_ids:
pixel_x = int(total_center[point_i - 1][0][0])
pixel_y = int(total_center[point_i - 1][0][1])
for p in pc2.read_points(ros_cloud,
field_names=("x", "y", "z"),
uvs=[[pixel_x, pixel_y]]):
# print('{} p.x: {}'.format(ids[point_i],p[0]))
# print('{} p.y: {}'.format(ids[point_i],p[1]))
# print('{} p.z: {}'.format(ids[point_i],p[2]))
pose_msg = Pose()
qx = total_rvec[point_i - 1][0][0]
qy = total_rvec[point_i - 1][0][1]
qz = total_rvec[point_i - 1][0][2]
if math.isnan(p[0]):
print('is nan')
p = [0, 0, 0]
p[0] = total_tvec[point_i - 1][0][0]
p[1] = total_tvec[point_i - 1][0][1]
p[2] = total_tvec[point_i - 1][0][2]
tx, ty, tz = transform_coor(p[0], p[1], p[2])
pose_msg.position.x = tx
pose_msg.position.y = ty
pose_msg.position.z = tz
pose_msg.orientation.x = qx
pose_msg.orientation.y = qy
pose_msg.orientation.z = qz
pose_msg.orientation.w = point_i
pose_arr.poses.append(pose_msg)
pose_pub.publish(pose_arr)
# When everything done, release the capture
except TypeError as e:
print(e)
return CameraRequestsResponse(pose_arr)
if not capture.isOpened:
print("Unable to open camera feed")
exit(0)
while True:
# Read a frame from 'capture' device
# return false if no frames has been grabbed
ret, frame = capture.read()
# ret = true if a frame has been grabbed, else false
# frame contains a Mat image (ready to be used with openCV)
# Ensure a frame has been grabbed
if frame is None:
break
# Detect aruco markers and register them in corners / ids (rejectedImgPoints is for debug)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame, aruco.getPredefinedDictionary(aruco.DICT_4X4_1000))
# aruco.estimatePoseSingleMarkers(corners, )
if ids is not None and len(ids) > 0:
aruco.drawDetectedMarkers(frame, corners, ids, (0, 0, 255))
print(rejectedImgPoints)
cv.imshow("Frame", frame)
keyboard = cv.waitKey(30)
if keyboard == 113:
break
# Default aruco dictionary
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
# Loop over the calibration images and detect markers
for subdir, dirs, files in os.walk('.\src\calibration\images'):
for file in files:
# Read into a variable
frame = cv2.imread(subdir + '\\' + file)
# Covert to grayscale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect the markers
corners, ids, rejected = aruco.detectMarkers(gray, aruco_dict)
if ids is not None:
# rvecs and tvecs are the rotation and translation vectors for each of the markers in corners.
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, 1, cameraMatrix, distCoeffs)
for rvec, tvec in zip(rvecs, tvecs):
aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvec, tvec, 1)
# Display the pose
cv2.imshow('frame', frame)
# Pause before the next frame
cv2.waitKey(3000)
