def get_corners_aruco(fname, board, skip=20):
cap = cv2.VideoCapture(fname)
length = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
allCorners = []
allIds = []
go = int(skip / 2)
board_type = get_board_type(board)
board_size = get_board_size(board)
max_size = get_expected_corners(board)
for framenum in trange(length, ncols=70):
ret, frame = cap.read()
if not ret:
break
if framenum % skip != 0 and go <= 0:
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
params = aruco.DetectorParameters_create()
params.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR
params.adaptiveThreshWinSizeMin = 100
params.adaptiveThreshWinSizeMax = 700
params.adaptiveThreshWinSizeStep = 50
params.adaptiveThreshConstant = 5
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, board.dictionary, parameters=params
)
if corners is None or len(corners) <= 2:
go = max(0, go - 1)
continue
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = aruco.refineDetectedMarkers(
gray, board, corners, ids, rejectedImgPoints, parameters=params
)
if board_type == "charuco" and len(detectedCorners) > 0:
ret, detectedCorners, detectedIds = aruco.interpolateCornersCharuco(
detectedCorners, detectedIds, gray, board
)
if (
detectedCorners is not None
and len(detectedCorners) >= 2
and len(detectedCorners) <= max_size
):
allCorners.append(detectedCorners)
allIds.append(detectedIds)
go = int(skip / 2)
go = max(0, go - 1)
cap.release()
return allCorners, allIds
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
camera_matrix, camera_distortion, _ = loadCameraParams('runcam_nano3')
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
cf = scf.cf
activate_high_level_commander(cf)
# We take off when the commander is created
with PositionHlCommander(scf) as pc:
#pc.go_to(0.0, 0.0, 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:
#-- 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 = 0 #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 the marker to follow
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,
ugly_const.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,
ugly_const.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)
cf.extpos.send_extpos(pos_cmd[0], pos_cmd[1],
pos_cmd[2])
print('Following tag ', id2follow, ' with position ',
pos_cmd[0], pos_cmd[1])
#if np.sqrt(pos_cmd[0]*pos_cmd[0]+pos_cmd[1]*pos_cmd[1]) > 0.05:
#mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, zvel, -att_camera[2]*ugly_const.Kyaw)
# elif pos_camera.item(2) > 0.1:
# mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, -0.05, -att_camera[2]*ugly_const.Kyaw)
# else:
# mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, 0.05, -att_camera[2]*ugly_const.Kyaw)
pc.go_to(0.0, 0.0)
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 multi_marker_pose():
'''
Function to perform pose estimation on ArUco markers
Return : rvec_list : list of estimated rotation vectors
tvec_list : list of estimated translation vectors
'''
cap1 = cv2.VideoCapture(0)
print('Calibrating....')
mtx, dist, _, _ = calib()
print('Calibrated')
dir_name = os.getcwd()
obj_1 = OBJ(os.path.join(dir_name, 'models/1.obj'), swapyz=True)
obj_2 = OBJ(os.path.join(dir_name, 'models/2.obj'), swapyz=True)
obj_3 = OBJ(os.path.join(dir_name, 'models/3.obj'), swapyz=True)
obj_4 = OBJ(os.path.join(dir_name, 'models/4.obj'), swapyz=True)
obj_5 = OBJ(os.path.join(dir_name, 'models/5.obj'), swapyz=True)
obj_6 = OBJ(os.path.join(dir_name, 'models/6.obj'), swapyz=True)
obj_7 = OBJ(os.path.join(dir_name, 'models/7.obj'), swapyz=True)
obj_8 = OBJ(os.path.join(dir_name, 'models/8.obj'), swapyz=True)
obj_9 = OBJ(os.path.join(dir_name, 'models/9.obj'), swapyz=True)
obj_10 = OBJ(os.path.join(dir_name, 'models/10.obj'), swapyz=True)
obj_11 = OBJ(os.path.join(dir_name, 'models/11.obj'), swapyz=True)
obj_12 = OBJ(os.path.join(dir_name, 'models/12.obj'), swapyz=True)
obj_13 = OBJ(os.path.join(dir_name, 'models/13.obj'), swapyz=True)
obj_14 = OBJ(os.path.join(dir_name, 'models/14.obj'), swapyz=True)
obj_15 = OBJ(os.path.join(dir_name, 'models/15.obj'), swapyz=True)
obj_16 = OBJ(os.path.join(dir_name, 'models/16.obj'), swapyz=True)
obj_17 = OBJ(os.path.join(dir_name, 'models/17.obj'), swapyz=True)
obj_18 = OBJ(os.path.join(dir_name, 'models/18.obj'), swapyz=True)
obj_19 = OBJ(os.path.join(dir_name, 'models/19.obj'), swapyz=True)
obj_20 = OBJ(os.path.join(dir_name, 'models/20.obj'), swapyz=True)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
params = aruco.DetectorParameters_create()
ref_1 = cv2.imread(os.path.join(dir_name, 'markers/m1.jpg'), 0)
corners_src1, ids_src1, rejected_src1 = aruco.detectMarkers(ref_1, aruco_dict,
parameters = params)
ref_2 = cv2.imread(os.path.join(dir_name, 'markers/m2.jpg'), 0)
corners_src2, ids_src2, rejected_src2 = aruco.detectMarkers(ref_2, aruco_dict,
parameters = params)
ref_3 = cv2.imread(os.path.join(dir_name, 'markers/m3.jpg'), 0)
corners_src3, ids_src3, rejected_src3 = aruco.detectMarkers(ref_3, aruco_dict,
parameters = params)
ref_4 = cv2.imread(os.path.join(dir_name, 'markers/m4.jpg'), 0)
corners_src4, ids_src4, rejected_src4 = aruco.detectMarkers(ref_4, aruco_dict,
parameters = params)
ref_5 = cv2.imread(os.path.join(dir_name, 'markers/m5.jpg'), 0)
corners_src5, ids_src5, rejected_src5 = aruco.detectMarkers(ref_5, aruco_dict,
parameters = params)
ref_6 = cv2.imread(os.path.join(dir_name, 'markers/m6.jpg'), 0)
corners_src6, ids_src6, rejected_src6 = aruco.detectMarkers(ref_6, aruco_dict,
parameters = params)
ref_7 = cv2.imread(os.path.join(dir_name, 'markers/m7.jpg'), 0)
corners_src7, ids_src7, rejected_src7 = aruco.detectMarkers(ref_7, aruco_dict,
parameters = params)
ref_8 = cv2.imread(os.path.join(dir_name, 'markers/m8.jpg'), 0)
corners_src8, ids_src8, rejected_src8 = aruco.detectMarkers(ref_8, aruco_dict,
parameters = params)
ref_9 = cv2.imread(os.path.join(dir_name, 'markers/m9.jpg'), 0)
corners_src9, ids_src9, rejected_src9 = aruco.detectMarkers(ref_9, aruco_dict,
parameters = params)
ref_10 = cv2.imread(os.path.join(dir_name, 'markers/m10.jpg'), 0)
corners_src10, ids_src10, rejected_src10 = aruco.detectMarkers(ref_10, aruco_dict,
parameters = params)
ref_11 = cv2.imread(os.path.join(dir_name, 'markers/m11.jpg'), 0)
corners_src11, ids_src11, rejected_src11 = aruco.detectMarkers(ref_11, aruco_dict,
parameters = params)
ref_12 = cv2.imread(os.path.join(dir_name, 'markers/m12.jpg'), 0)
corners_src12, ids_src12, rejected_src12 = aruco.detectMarkers(ref_12, aruco_dict,
parameters = params)
ref_13 = cv2.imread(os.path.join(dir_name, 'markers/m13.jpg'), 0)
corners_src13, ids_src13, rejected_src13 = aruco.detectMarkers(ref_13, aruco_dict,
parameters = params)
ref_14 = cv2.imread(os.path.join(dir_name, 'markers/m14.jpg'), 0)
corners_src14, ids_src14, rejected_src14 = aruco.detectMarkers(ref_14, aruco_dict,
parameters = params)
ref_15 = cv2.imread(os.path.join(dir_name, 'markers/m15.jpg'), 0)
corners_src15, ids_src15, rejected_src15 = aruco.detectMarkers(ref_15, aruco_dict,
parameters = params)
ref_16 = cv2.imread(os.path.join(dir_name, 'markers/m16.jpg'), 0)
corners_src16, ids_src16, rejected_src16 = aruco.detectMarkers(ref_16, aruco_dict,
parameters = params)
ref_17 = cv2.imread(os.path.join(dir_name, 'markers/m17.jpg'), 0)
corners_src17, ids_src17, rejected_src17 = aruco.detectMarkers(ref_17, aruco_dict,
parameters = params)
ref_18 = cv2.imread(os.path.join(dir_name, 'markers/m18.jpg'), 0)
corners_src18, ids_src18, rejected_src18 = aruco.detectMarkers(ref_18, aruco_dict,
parameters = params)
ref_19 = cv2.imread(os.path.join(dir_name, 'markers/m19.jpg'), 0)
corners_src19, ids_src19, rejected_src19 = aruco.detectMarkers(ref_19, aruco_dict,
parameters = params)
ref_20 = cv2.imread(os.path.join(dir_name, 'markers/m20.jpg'), 0)
corners_src20, ids_src20, rejected_src20 = aruco.detectMarkers(ref_20, aruco_dict,
parameters = params)
count = 0
while(1):
_, frame = cap1.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(gray, aruco_dict,
parameters = params)
if np.all(ids != None):
rvec_list = []
tvec_list = []
aruco.drawDetectedMarkers(frame, corners)
for i in range(len(ids)):
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners[i], 0.05, mtx, dist)
#aruco.drawAxis(frame, mtx, dist, rvecs[0], tvecs[0], 0.05)
cv2.putText(frame, str(ids[i][0]),
tuple(corners[i][0][2]),
font, 0.5, (0, 0, 255), 1, 4)
rvec_list.append(rvecs)
#print(tvecs[0][0])
tvec_list.append(tvecs)
if ids == ids_src1:
homography, mask = cv2.findHomography(corners_src1[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_1, projection, ref_1, False)
except:
pass
elif ids == ids_src2:
homography, mask = cv2.findHomography(corners_src2[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_2, projection, ref_2, False)
except:
pass
elif ids == ids_src3:
homography, mask = cv2.findHomography(corners_src3[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_3, projection, ref_3, False)
except:
pass
elif ids == ids_src4:
homography, mask = cv2.findHomography(corners_src4[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_4, projection, ref_4, False)
except:
pass
elif ids == ids_src5:
homography, mask = cv2.findHomography(corners_src5[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_5, projection, ref_5, False)
except:
pass
elif ids == ids_src6:
homography, mask = cv2.findHomography(corners_src6[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_6, projection, ref_6, False)
except:
pass
elif ids == ids_src7:
homography, mask = cv2.findHomography(corners_src7[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_7, projection, ref_7, False)
except:
pass
elif ids == ids_src8:
homography, mask = cv2.findHomography(corners_src8[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_8, projection, ref_8, False)
except:
pass
elif ids == ids_src9:
homography, mask = cv2.findHomography(corners_src9[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_9, projection, ref_9, False)
except:
pass
elif ids == ids_src10:
homography, mask = cv2.findHomography(corners_src10[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_10, projection, ref_10, False)
except:
pass
elif ids == ids_src11:
homography, mask = cv2.findHomography(corners_src11[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_11, projection, ref_11, False)
except:
pass
elif ids == ids_src12:
homography, mask = cv2.findHomography(corners_src12[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_12, projection, ref_12, False)
except:
pass
elif ids == ids_src13:
homography, mask = cv2.findHomography(corners_src13[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_13, projection, ref_13, False)
except:
pass
elif ids == ids_src14:
homography, mask = cv2.findHomography(corners_src14[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_14, projection, ref_14, False)
except:
pass
elif ids == ids_src15:
homography, mask = cv2.findHomography(corners_src15[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_15, projection, ref_15, False)
except:
pass
elif ids == ids_src16:
homography, mask = cv2.findHomography(corners_src16[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_16, projection, ref_16, False)
except:
pass
elif ids == ids_src17:
homography, mask = cv2.findHomography(corners_src17[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_17, projection, ref_17, False)
except:
pass
elif ids == ids_src18:
homography, mask = cv2.findHomography(corners_src18[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_18, projection, ref_18, False)
except:
pass
elif ids == ids_src19:
homography, mask = cv2.findHomography(corners_src19[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_19, projection, ref_19, False)
except:
pass
elif ids == ids_src20:
homography, mask = cv2.findHomography(corners_src20[0], corners[0], cv2.RANSAC, 5.0)
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
projection = projection_matrix(mtx, homography)
# project cube or model
frame = render(frame, obj_20, projection, ref_20, False)
except:
pass
img = cv2.imread('clk4.png', 1)
img1 = cv2.imread('calendar3.png', 1)
img2 = cv2.imread('tv.png', 1)
rows2, cols2, _ = img2.shape
rows, cols, xyz = img.shape
row, col, xsd = img1.shape
date = time.ctime()
if (count % 400==0):
info,c_t_abs,c_t,c_scores = showmatch()
img = cv2.putText(img, str(str(date).split(" ")[3]), (15,40), cv2.FONT_HERSHEY_SCRIPT_SIMPLEX , 0.7, (0,0,0),1)
img2 = cv2.putText(img2, "LIVE" , (20,22), cv2.FONT_HERSHEY_DUPLEX, 0.5, (0,0,255), 1)
img2 = cv2.putText(img2, c_t[0].text+' VS '+c_t[1].text, (70,22), cv2.FONT_HERSHEY_DUPLEX, 0.5, (0,0,0), 1)
img2 = cv2.putText(img2, c_t_abs[0].text+' '+c_scores[0].text, (20,44), cv2.FONT_HERSHEY_DUPLEX, 0.45, (0,0,0), 1)
img2 = cv2.putText(img2, c_t_abs[1].text+' '+c_scores[1].text, (20,60), cv2.FONT_HERSHEY_DUPLEX, 0.45, (0,0,0), 1)
# img1 = cv2.putText(img1, str(str(date).split(" ")[4]), (30,85), cv2.FONT_HERSHEY_SCRIPT_SIMPLEX , 0.7, (255,0,0),1)
# img1 = cv2.putText(img1, str(str(date).split(" ")[1]), (15,60), cv2.FONT_HERSHEY_SCRIPT_SIMPLEX , 0.7, (255,0,0),1)
# img1 = cv2.putText(img1, str(str(date).split(" ")[2]), (70,60), cv2.FONT_HERSHEY_SCRIPT_SIMPLEX , 0.7, (255,0,0),1)
frame = cv2.resize(frame,(950,980),interpolation = cv2.INTER_AREA)
#frame = cv2.putText(frame,voice,(600,400), cv2.FONT_HERSHEY_SCRIPT_SIMPLEX , 0.7, (255,0,0),1)
modification2 = cv2.addWeighted(img2, 0.8, frame[600:600+rows2, 40:40+cols2], 0.7, 2)
modification = cv2.addWeighted(img, 0.3, frame[60:60+rows, 40:40+cols], 0.7, 2)
frame[60:60+rows, 40:40+cols] = modification
frame[600:600+rows2, 40:40+cols2] = modification2
count = count+1
# modification1 = cv2.addWeighted(img1, 0.6, frame[55:55+row,800:800+col], 0.4, 2)
# frame[40:40+row, 800:800+col] = modification1
cv2.imshow('Pose Estimation', frame)
if cv2.waitKey(1) == ord('q'):
break
cap1.release()
cv2.destroyAllWindows()
return rvec_list, tvec_list
def deleteframes(self, team_id, file_name, contours, flag=True):
count = 0
#name = "team_id_" + str(team_id[0]) + ".png"
name = "team_id_" + str(self.num) + ".png"
#print(name)
cap = cv2.VideoCapture(file_name)
flag_pm = 0
li_pm = []
count_pm = 0
pm_list = []
start = 0
while (cap.isOpened()):
#print("I am in deleteframes")
ret, image = cap.read()
if ret == False:
break
tl = 0
tr = 0
bl = 0
br = 0
tlx = 0
tly = 0
blx = 0
bly = 0
trix = 0
triy = 0
brx = 0
bry = 0
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
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):
flag = True
#print("printing frame" + str(count))
#count += 1
#flag_pm,li_pm,count_pm,pm_list,start = self.physical_marker(image,flag_pm=flag_pm,li_pm=li_pm,count_pm=count_pm,pm_list=pm_list,start=start)
warped_frame = self.warping(image, contours)
self.filter_top_of_robot(warped_frame)
elif (tlx, tly, trix, triy, blx, bly, brx, bry) == (0, 0, 0, 0, 0,
0, 0, 0):
if flag == True:
count += 1
print("count = " + str(count))
flag = False
#cv2.imshow("Original", image)
#cv2.waitKey(1)
cap.release()
cv2.destroyAllWindows()
#print("i am out of delete_frames")
cv2.imwrite(name, self.img)
#cv2.imwrite("circleplot.png", img2)
return (name, count, self.img, img_with_circles) #,count_pm,pm_list)
from imutils.video import VideoStream
import time
import sys
from calibration import calibrate
import os
print("[INFO] Use Ctrl+C to exit.")
print("[INFO] calibrating camera...")
ret, camera_matrix, dist_coeffs = calibrate()
if ret:
print("[INFO] attained camera calibration values.")
else:
print("[ERROR] failed to get camera calibration values...")
arucoDict = auo.Dictionary_get(auo.DICT_6X6_1000)
arucoParams = auo.DetectorParameters_create()
print("[INFO] starting video stream...")
vs = VideoStream(src=0).start()
time.sleep(2.0)
while True:
# grab the frame from the threaded video stream and resize it
# to have a maximum width of 1000 pixels with , width=1000.
frame = vs.read()
frame = imutils.resize(frame)
# detect ArUco markers in the input frame
(corners, ids, rejected) = auo.detectMarkers(frame,
arucoDict,
parameters=arucoParams)
if len(corners) > 0:
import std_msgs, std_srvs
import numpy as np
import cv2
import cv2.aruco as aruco
import os
import pickle
from aruco_hand_eye.srv import aruco_info, aruco_infoResponse
import time
import pyrealsense2 as rs
NUMBER = 5
# # Constant parameters used in Aruco methods
# ARUCO_PARAMETERS = aruco.DetectorParameters_create()
# ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_100)
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_50)
CHARUCOBOARD_ROWCOUNT = 7
CHARUCOBOARD_COLCOUNT = 5
# Create grid board object we're using in our stream
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=CHARUCOBOARD_COLCOUNT,
squaresY=CHARUCOBOARD_ROWCOUNT,
squareLength=0.0322,
markerLength=0.0216,
# squareLength=0.04,
# markerLength=0.02,
dictionary=ARUCO_DICT)
def detect_motion(frameCount):
# grab global references to the video stream, output frame, and
# lock variables
global vs, outputFrame, lock
# times = []
# loop over frames from the video stream
while True:
# Start time
# start = time.time()
ret, frame = vs.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_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
# check if the ids list is not empty
# if no check is added the code will crash
if np.all(ids != None):
# estimate pose of each marker and return the values
# rvet and tvec-different from camera coefficients
rvec, tvec = aruco.estimatePoseSingleMarkers(
corners, 0.05, mtx, dist)
# (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)
# 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)
# end = time.time()
#
# times.append(end - start)
#
# if len(times) > 10:
# times = times[:9]
#
# cv2.putText(frame, f"FPS: {round(len(times) / sum(times))}", (0, 100), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
# acquire the lock, set the output frame, and release the
# lock
with lock:
outputFrame = frame.copy()
def main(marker_length, marker_separation):
# opencv/aruco settings
cam = cv2.VideoCapture(0)
ardict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
board = aruco.GridBoard_create(4, 5, marker_length, marker_separation,
ardict)
parameters = aruco.DetectorParameters_create()
parameters.minMarkerPerimeterRate = 0.03
# load camera parameters
with open("calib.json", 'r') as f:
data = json.load(f)
dr = DrawClass(data.get('camera_matrix'), data.get('dist_coeff'))
# and make some variables for aruco
mtx = dr.mtx
dist = dr.dist
# print(cv2.calibrationMatrixValues(mtx, (640, 480), 3.6, 2.7))
# globals variables
cs = CoordinatesSystem()
while cam.isOpened():
k = cv2.waitKey(16)
ret, frame = cam.read()
info_screen = np.ones((500, 1000, 3), np.uint8)
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
corners, ids, rejected_img_points = aruco.detectMarkers(
gray, ardict, parameters=parameters)
if corners is not None:
frame = dr.draw_markers(frame, corners, ids)
r_vecs, t_vecs, _ = aruco.estimatePoseSingleMarkers(
corners, marker_length, mtx, dist)
markers = [
Marker(ids[i][0], r_vecs[i], t_vecs[i])
for i in range(len(corners))
]
frame = dr.draw_axis_of_markers(frame, markers, 1)
info_screen = dr.print_text(info_screen, markers)
if len(markers) == 2:
# rv, tv = relative_position(markers[0].r_vec, markers[0].t_vec, markers[1].r_vec, markers[1].t_vec)
tv = relative_position2(markers[0].r_vec, markers[0].t_vec,
markers[1].t_vec)
info_screen = dr.draw_debug_lines(info_screen, tv)
print(tv)
if len(markers) == 1 and k == ord('p'):
cs.reset(markers[0].r_vec, markers[0].t_vec)
print(cs.rot_matrix)
if len(markers) == 1 and cs.fcs_r_vec is not None:
tv3 = cs.get_coordinates_in_cs(markers[0].t_vec)
frame = dr.draw_axis_points(frame, cs.fcs_r_vec, cs.fcs_t_vec)
info_screen = dr.draw_debug_lines(info_screen, tv3)
print(tv3)
cv2.line(frame, (320, 240), (320, 240), (0, 0, 0), 4)
cv2.imshow("frame", frame)
cv2.imshow("info", info_screen)
if k == ord('q'):
cam.release()
break
def main():
ser = serial.Serial('COM3', 115200, timeout=0)
print(ser.name)
# targetState = (20,80,0)
robotNode = 1
robotTargetLoc1 = (80, 20)
robotMap = []
# robotLocation2 = (60,70)
globalMap = initializeGraph()
mapping.plotGraph(robotMap, [robotTargetLoc1])
performRobotWriting = True
#Holds the values of the four corners of the robotic environment. Goes clockwise starting with the top left corner
#The ids for the aruco tags for each of the four corners goes from 0 to 3 clockwise, starting with the top left corner
fourCorners = [[], [], [], []]
vc = cv2.VideoCapture(1)
# np.load("sample_images.npz")
with np.load("sample_images.npz") as data:
mtx = data['mtx']
dist = data['dist']
# ret,frame = vc.read()
# image = frame
# time.sleep(1)
# ret,frame = vc.read()
# image = frame
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
iter = 0
node = 1
x = ''
y = ''
a = ''
initialized = False
initialxya = []
iter = 0
while (vc.isOpened()):
# if iter % 90 == 0:
# plt.pause(0.001)
iter = (iter + 1) % 25
ret, frame = vc.read()
image = frame
# image = cv2.imread("52814747.png")
# image = cv2.imread("arucoTestImage.png")
# aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
image, aruco_dict, parameters=parameters)
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, 0.1, mtx, dist)
rotations = []
if ids is not None:
for i in range(len(ids)):
if ids[i] == 5:
if rvec[i][0][0] > 0:
# rvec[i][0][0] = -1 * rvec[i][0][0]
image = aruco.drawAxis(image, mtx, dist, rvec[i],
tvec[i], 0.1)
a = rvec[i][0][1]
# print(rvec[i])
a = objectLocalization.convertAtoRadians(a)
# if ids[i] < 5:
# print("Drawing ID: " + str(ids[i]) )
# print("before")
# print(rvec[i])
# image = aruco.drawAxis(image,mtx,dist,rvec[i],tvec[i],0.1)
# print("after")
if ids is not None:
for i in range(len(ids)):
if ids[i] == 0:
fourCorners[0] = objectLocalization.getRectMid(
corners[i][0])
elif ids[i] == 1:
fourCorners[1] = objectLocalization.getRectMid(
corners[i][0])
elif ids[i] == 2:
fourCorners[2] = objectLocalization.getRectMid(
corners[i][0])
elif ids[i] == 3:
fourCorners[3] = objectLocalization.getRectMid(
corners[i][0])
elif ids[i] == 5:
if (len(fourCorners[0]) > 0 and len(fourCorners[1]) > 0
and len(fourCorners[2]) > 0
and len(fourCorners[3]) > 0):
[x, y] = objectLocalization.scalePoint(
fourCorners,
objectLocalization.getRectMid(corners[i][0]))
[x,
y] = objectLocalization.convertToRobotLocation(x, y)
print(fourCorners)
# print(corners)
# print(ids)
# print(corners, ids, rejectedImgPoints)
#NOTE: Two lines below used to draw markers originally
# aruco.drawDetectedMarkers(image, corners, ids)
# aruco.drawDetectedMarkers(image, rejectedImgPoints, borderColor=(100, 0, 240))
cv2.imshow('Video', image)
key = cv2.waitKey(20)
if key == 27:
# exit on ESC
break
# iter = 0
# while (iter < 5):
temp = ser.read(1000)
# temp = ser.readline()
if temp != b'':
if temp == b'q' or temp == 'Q':
ser.close()
break
# print(temp)
# if temp[0] == '$':
if len(temp) > 0:
RFIDinfo = parseRFIDInfo(temp)
print("RFID Info: ")
print(RFIDinfo)
# print("RFIDInfo: " + str(RFIDinfo))
if RFIDinfo != None:
if mapping.euclideanDistance(
(x, y), (RFIDinfo[0][0], RFIDinfo[0][1])
) < 10 and mapping.euclideanDistance(
(x, y), robotTargetLoc1
) < 10: #Make sure we're in the range of the tag we think we are
(newTargetLoc, info, preprocessedMap
) = mapping.updateMapandDetermineNextStep(
robotMap, globalMap, robotTargetLoc1, RFIDinfo)
print("------------------" + str(newTargetLoc) +
"---------------------")
if performRobotWriting:
message = prepRFIDInfo(robotTargetLoc1, info,
preprocessedMap)
message = message + "\r\n"
ser.write(str.encode(message))
print("***********Writing Message*************")
print(message)
ser.flush()
mapping.plotGraph(preprocessedMap, [robotTargetLoc1])
robotTargetLoc1 = newTargetLoc
# mapping.plotGraph(robotMap,[robotTargetLoc1])
# if len(code) >= len(shortestSample):
# # if not xVals and not yVals:
# tempState = (int(xVals),int(yVals),0)
# if tempState != (0,0,0):
# targetState = tempState
# print("------------- NEW TARGET STATE ----------")
# print(targetState)
first = True
job = multiprocessing.Process(target=plotMap, args=(robotMap, (x, y)))
# time.sleep(0.1)
# print(x == '')
# print(y == '')
# print(a)
# time.sleep(0.05)
if (x != '' and y != '' and a != ''):
# print("x: " + str(x))
# print("y: " + str(y))
#
# plt.pause(0.001)
# job.start()
# job.join()
# if iter % 45 == 0:
# job.start()
# mapping.plotGraph(robotMap,[(x,y)])
# plotMap(robotMap,(x,y))
# if first:
# job.start()
if (
len(initialxya) < 10
): #looking to find the average of the first several measurements to get an accurate starting point
initialxya.append((x, y, a))
else:
if not initialized:
avex = 0
avey = 0
avea = 0
#NOTE: Possible bug could occur if the robot is aligned along 0 degrees since this could fluctuate
#between 0 and 2 pi for values (resulting in an average of pi, the exact oppposite direction)
for i in range(10):
avex = avex + initialxya[i][0]
avey = avey + initialxya[i][1]
avea = avea + initialxya[i][2]
avex = avex / len(initialxya)
avey = avey / len(initialxya)
avea = avea / len(initialxya)
prevxya = (avex, avey, avea)
prevxya2 = (avex, avey, avea)
initialized = True
# print(initialxya)
previouslyMissed = False
falsePrev = prevxya
else:
# if withinRange(a,prevxya2[2], math.pi * 1/ 2) or (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed): #Make sure the angle doesn't flip an unreasonable amount
# if withinRange(a,prevxya2[2], math.pi * 1/ 2) or (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed): #Make sure the angle doesn't flip an unreasonable amount
# print("Target State: " + str(targetState))
# print("A: " + str(a))
if (True):
# if (not withinRange(a,prevxya[2] - math.pi,math.pi*4/8)):
# if (withinRange(a,prevxya2[2], math.pi * 3/ 4) and withinRange(a,prevxya[2], math.pi * 2/4)) or (previouslyMissed and withinRange(a,falsePrev[2],math.pi * 1 /8)): #3/4 1/2Make sure the angle doesn't flip an unreasonable amount
message = robotControl.prepareControlMessage(
(x, y, a), robotTargetLoc1)
message = "$" + str(robotNode) + str(
message) + '\r\n' #preappend the robot node number
ser.write(str.encode(message))
# print(message)
ser.flush()
# if (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed):
# prevxya2 = falsePrev
# else:
# prevxya2 = prevxya
prevxya2 = prevxya
prevxya = (x, y, a)
# print(initialxya)
# print((avex,avey,avea))
# ser.close()
# time.sleep(0.1)
# print("Just flushed")
else:
# print("MISSED IT")
# print(prevxya)
# print(prevxya2)
# print(x)
# print(y)
# print("A: " + str(a))
falsePrev = (x, y, a)
previouslyMissed = True
cv2.waitKey(0)
cv2.destroyAllWindows()
message = "$" + str(
robotNode) + "f 0\r\n" #preappend the robot node number
ser.write(str.encode(message))
ser.close()
def main():
global aruco_and_middle, stop_pls, filtered
color = (255, 255, 0)
# Set aruco sizes and then load the dictionaries
if six_by_six:
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
else:
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
# Keep running
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
b = time.time()
image = frame.array
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
marker = aruco.drawDetectedMarkers(gray,
corners,
ids,
borderColor=color)
erdil = cv2.erode(gray, kernel, iterations=5)
erdil = cv2.dilate(erdil, kernel, iterations=5)
mask = cv2.inRange(erdil, lower, upper)
mask = cv2.bitwise_not(mask)
ret, thresh = cv2.threshold(mask, 255, 255, 255)
contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
res = cv2.bitwise_and(image, image, mask=mask)
middle = -1
middles = []
if ids is None:
for cnt in contours:
x, y, w, h = cv2.boundingRect(cnt)
middles.append(x + w / 2.0)
else:
for corner in corners[0][0]:
middles.append(corner[0])
if (len(middles) > 0):
middle = sum(middles) / len(middles)
else:
middle = -1
if filtered is None:
filtered = middle
# Exponential moving average filter
else:
filtered = 0.7 * middle + 0.3 * filtered
send = round(concat(filtered), 3)
# Set the global variable
aruco_and_middle = (ids, send)
if ids is not None:
# Used for statistics
e = time.time()
avg.append(e - b)
# The image displays
cv2.drawContours(res, contours, -1, (0, 255, 0), 3)
res = cv2.circle(res, (int(filtered), 100), 5, color, 3)
cv2.imshow("Marker", marker)
cv2.imshow("Result", res)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
if key == ord("q"):
print("Average aruco detection:", sum(avg) / len(avg))
break
def get_transform(self):
#aruco width and height
ret, self.aruco_frame = self.video.read()
gray = cv2.cvtColor(self.aruco_frame, cv2.COLOR_BGR2GRAY)
T = cv2.getTrackbarPos('T','trackbars')
retval, thesh = cv2.threshold(gray,T,255,cv2.THRESH_BINARY)
cv2.imshow("Thresh",thesh)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
parameters = aruco.DetectorParameters_create()
#lists of ids and the corners beloning to each ids
corners, ids, rejectedImgPoints = aruco.detectMarkers(thesh, aruco_dict, parameters=parameters)
self.aruco_frame = aruco.drawDetectedMarkers(self.aruco_frame, corners,ids,(255,255,0))
platform_center_x = 0
platform_center_y = 0
angle = 0
#If an aruco marker is found
if np.all(ids != None):
#For all aruco markers found
for i in range(0,int(ids.size)):
if ids[0][i] == 34:
aruco_x_coor,aruco_y_coor,angle,conversion_factor = self.__get_aruco_coor_angle(corners,i)
#Distance from the center of the aruco marker to the centre of the object in mm
mach_distance_x = 1#200
mach_distance_y = 800#480
#distance from the center of the aruco marker to the bottom left corner of the platform
distance_offset = 0
station_center_x,station_center_y = self.__get_object_center_from_aruco(aruco_x_coor,aruco_y_coor,angle,mach_distance_x,mach_distance_y,distance_offset,conversion_factor)
station_height = 200*conversion_factor
station_width = 200*conversion_factor
self.__detect_blocks_in_roi(station_center_x,station_center_y,angle,conversion_factor,station_height,station_width)
elif ids[0][i] == 43:
plat_distance_x = 200
plat_distance_y = 480
distance_offset = 50
aruco_x_coor,aruco_y_coor,angle,conversion_factor = self.__get_aruco_coor_angle(corners,i)
platform_center_x,platform_center_y = self.__get_object_center_from_aruco(aruco_x_coor,aruco_y_coor,angle,plat_distance_x,plat_distance_y,distance_offset,conversion_factor)
platform_height = 500*conversion_factor
platform_width = 550*conversion_factor
self.__mask_object_with_aruco_coor(platform_center_x,platform_center_y,angle,conversion_factor,platform_height,platform_width)
else:
print("Unrecongnised ArUco marker")
found = 1
transform_dict = {
"state" : found,
"x" : platform_center_x,
"y" : platform_center_y,
"angle" : angle
}
return (transform_dict)
else:
found = 0
transform_dict = {
"state" : found,
"x" : 0,
"y" : 0,
"angle" : 0
}
return (transform_dict)
def track_truck(rp):
cap = cv2.VideoCapture(1)
w = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
# print w
# print h
d_pos = 1 #distance difference between cb and truck
while True:
ret, frame = cap.read()
if ret == False:
continue
framebw = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
parameters = aruco.DetectorParameters_create()
corners, ids, _ = aruco.detectMarkers(framebw,
aruco_dict,
parameters=parameters)
# print ids
i = ids.tolist().index([0]) #gives total aruco markers
# print i
l = 0
l1 = 0
z = int(ids[i][0])
# print z
cv2.circle(frame, (corners[i][0][0][0], corners[i][0][0][1]), 5,
(125, 125, 125), -1)
cv2.circle(frame, (corners[i][0][1][0], corners[i][0][1][1]), 5,
(0, 255, 0), -1)
cv2.circle(frame, (corners[i][0][2][0], corners[i][0][2][1]), 5,
(180, 105, 255), -1)
cv2.circle(frame, (corners[i][0][3][0], corners[i][0][3][1]), 5,
(255, 255, 255), -1)
x = ((corners[i][0][0][0] + corners[i][0][2][0]) / 2)
y = ((corners[i][0][0][1] + corners[i][0][2][1]) / 2)
cv2.circle(frame, (int(x), int(y)), 5, (255, 0, 0), -1) #center
x1 = (corners[i][0][0][0] + corners[i][0][1][0]) / 2
y1 = (corners[i][0][0][1] + corners[i][0][1][1]) / 2
cv2.circle(frame, (int(x1), int(y1)), 5, (0, 0, 255),
-1) #midpoint of starting edge
cv2.line(frame, (int(x), int(y)), (int(x1), int(y1)), (255, 0, 0), 3)
a = y1 - y
b = x1 - x
theta = 3.14 - math.atan2(
a, b
) #taking negative to the resultant angle .Angle lies between [-pi,pi]
# print theta
# print '........'
font = cv2.FONT_HERSHEY_SIMPLEX
q = (
x - 320
) * 0.003351064 #0.003351064 #(x-320)*0.00303077335822 #0.00353077335822ipx along x axis=0.003515625 m
r = (
y - 240
) * 0.003351064 #0.003389831 #(y-240)*0.00302814921311 #0.00352814921311ipx along y axis=0.0034375 m
emptyBuff = bytearray()
position = [-q, r, +4.1000e-02]
orientation = [0, 0, theta]
# print position
# print orientation
x1 = position[0] - rp[0]
y1 = position[1] - rp[1]
d_pos = math.sqrt((x1)**2 + (y1)**2)
if d_pos < 0.015:
break
cap.release()
cv2.destroyAllWindows()
return
def setobj(flag):
cap = cv2.VideoCapture(1)
w = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
# print w
# print h
time.sleep(0)
while True:
ret, frame = cap.read()
framebw = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
parameters = aruco.DetectorParameters_create()
corners, ids, _ = aruco.detectMarkers(framebw,
aruco_dict,
parameters=parameters)
# print ids
t = len(ids) #gives total aruco markers
# print t
l = 0
l1 = 0
for i in range(0, t):
z = int(ids[i][0])
# print'>>>>>>>'
cv2.circle(frame, (corners[i][0][0][0], corners[i][0][0][1]), 5,
(125, 125, 125), -1)
cv2.circle(frame, (corners[i][0][1][0], corners[i][0][1][1]), 5,
(0, 255, 0), -1)
cv2.circle(frame, (corners[i][0][2][0], corners[i][0][2][1]), 5,
(180, 105, 255), -1)
cv2.circle(frame, (corners[i][0][3][0], corners[i][0][3][1]), 5,
(255, 255, 255), -1)
x = ((corners[i][0][0][0] + corners[i][0][2][0]) / 2)
y = ((corners[i][0][0][1] + corners[i][0][2][1]) / 2)
cv2.circle(frame, (int(x), int(y)), 5, (255, 0, 0), -1) #center
x1 = (corners[i][0][0][0] + corners[i][0][1][0]) / 2
y1 = (corners[i][0][0][1] + corners[i][0][1][1]) / 2
cv2.circle(frame, (int(x1), int(y1)), 5, (0, 0, 255),
-1) #midpoint of starting edge
cv2.line(frame, (int(x), int(y)), (int(x1), int(y1)), (255, 0, 0),
3)
a = y1 - y
b = x1 - x
theta = 3.14 - math.atan2(
a, b
) #taking negative to the resultant angle .Angle lies between [-pi,pi]
# print theta
# print '........'
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(frame, str(theta), (int(x + 100), int(y - 20)), font,
0.8, (0, 255, 0), 2, cv2.LINE_AA)
q = (
x - 320
) * 0.003351064 #0.003351064 #(x-320)*0.00303077335822 #0.00353077335822ipx along x axis=0.003515625 m
r = (
y - 240
) * 0.003351064 #0.003389831 #(y-240)*0.00302814921311 #0.00352814921311ipx along y axis=0.0034375 m
emptyBuff = bytearray()
orientation = [0, 0, theta]
if z == 0:
position = [-q, r, +4.1000e-02]
returnCode1, outInts1, path_pos, outStrings1, outBuffer1 = vrep.simxCallScriptFunction(
clientID, 'script', vrep.sim_scripttype_childscript,
'shift', [], position, [], emptyBuff,
vrep.simx_opmode_blocking)
returnCode1, outInts1, path_pos, outStrings1, outBuffer1 = vrep.simxCallScriptFunction(
clientID, 'script', vrep.sim_scripttype_childscript,
'rotate', [], orientation, [], emptyBuff,
vrep.simx_opmode_blocking)
# print '0303030303'
else:
if flag == 0:
if z in freshfruits_id:
position = [-q, r, +4.1000e-02]
if position[0] > 0:
if position[1] > 0:
returnCode = vrep.simxSetObjectPosition(
clientID, freshfruits[0], -1, position,
vrep.simx_opmode_oneshot_wait)
if position[1] < 0:
returnCode = vrep.simxSetObjectPosition(
clientID, freshfruits[1], -1, position,
vrep.simx_opmode_oneshot_wait)
if position[0] < 0:
returnCode = vrep.simxSetObjectPosition(
clientID, freshfruits[2], -1, position,
vrep.simx_opmode_oneshot_wait)
# print returnCode
# print r
# print -q
# print '03030330-------------'
if z in damagedfruits_id:
position = [-q, r, +4.1000e-02]
returnCode = vrep.simxSetObjectPosition(
clientID, damagedfruits[l1], -1, position,
vrep.simx_opmode_oneshot_wait)
# print returnCode
# print r
# print -q
# print '03030330-------------'
l1 = l1 + 1
flag = flag + 1
cv2.imshow('frame', frame)
if i == t - 1:
cap.release()
cv2.destroyAllWindows()
return
def cv_Demo2():
rawCapture = PiRGBArray(camera)
camera.iso = 100
# Wait for the automatic gain control to settle
time.sleep(2)
global angle
global marker_distance_cm
# Now fix the values
camera.shutter_speed = camera.exposure_speed
camera.exposure_mode = 'off'
g = camera.awb_gains
camera.awb_mode = 'off'
camera.awb_gains = g
# allow the camera to warmup
time.sleep(0.1)
camera.framerate = 32
aruco_dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
timer = 0
xwidth = 54 * (math.pi / 180)
ywidth = 41 * (math.pi / 180)
marker_flag = 1
# start continuous picture taking
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
marker_found = False
# convert to grayscale
image = frame.array
cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
image, aruco_dictionary, parameters=parameters)
if type(ids) == numpy.ndarray:
print("Marker found")
marker_found = True
output_array = numpy.ndarray(len(ids))
index = 0
for i in ids:
for j in i:
output_array[index] = j
print("Marker Indexes:", end=" ")
print(j)
#Determine angle of marcker form the center of the camera
xcenter = int(
(corners[0][0][0][0] + corners[0][0][2][0]) / 2)
ycenter = int(
(corners[0][0][0][1] + corners[0][0][2][1]) / 2)
yheight = abs(
int(corners[0][0][0][1] - corners[0][0][2][1]))
marker_distance_ft = 490 / yheight
print(yheight)
print("Marker Distance in ft: ", end="")
print(marker_distance_ft)
print("Rounding: ", end="")
print(round(marker_distance_ft))
print(marker_distance_cm)
# marker_distance_cm = [int(ele) for ele in str(marker_distance_cm) if ele.isdigit()]
#print("Marker Distacne in m: ", end="")
#print(marker_distance_m)
imageMiddleX = image.shape[1] / 2
xdist = (xcenter - image.shape[1] / 2)
ydist = (ycenter - image.shape[0] / 2)
xangle = (xdist / image.shape[1]) * xwidth
yangle = (ydist / image.shape[0]) * ywidth
# Calculate the angle from the z-axis to the center point
# First calculate distance (in pixels to screen) on z-axis
a1 = xdist / math.tan(xangle)
a2 = ydist / math.tan(yangle)
a = (a1 + a2) / 2
distance = math.sqrt(pow(xdist, 2) + pow(ydist, 2))
#Calculate final angle for each Aruco
angle = math.atan2(distance, a) * (180 / math.pi)
print("Correction: ", end="")
marker_distance_ft = marker_distance_ft * 0.96 - (
-2.5244 * pow(angle * math.pi / 180, 2) + 0.027)
print(marker_distance_ft)
marker_distance_cm = marker_distance_ft * 30.48 * 100
marker_distance_cm = int(marker_distance_cm)
if (xcenter > imageMiddleX):
angle *= -1
if (angle < -3.8 and angle > -4.3):
angle += 4
else:
angle += 4
if (angle > 0):
angle -= 2
display_angle = str(angle)
print("Angle from camera:", angle, "degrees")
lcd.message = ("Beacon Detected \nAngle: %s" %
(display_angle))
marker_flag = 1
index += 1
print()
break
rawCapture.truncate(0)
if marker_found == False:
print("No markers found")
if marker_flag == 1:
lcd.clear()
marker_flag = 0
lcd.message = ("Beacon Not\nDetected")
else:
break
def vision():
# Load the camera calibration values
Camera = np.load('Calibration_Surface_Book_Rear_Camera.npz')
CM = Camera['CM'] # camera matrix
dist_coef = Camera['dist_coef'] # distortion coefficients from the camera
aruco_dict = aruco.Dictionary_get(
aruco.DICT_4X4_50) # Load the aruco dictionary
pa = aruco.DetectorParameters_create() # Set the detection parameters
# Select the correct camera (0) = front camera, (1) = rear camera
cap = cv2.VideoCapture(1)
# Set the width and heigth of the camera to 640x480
cap.set(3, 640)
cap.set(4, 480)
# Create two opencv named windows
cv2.namedWindow("frame-image", cv2.WINDOW_AUTOSIZE)
# Position the window
cv2.moveWindow("frame-image", 0, 0)
t_end = time.time() + 10
# Execute this continuously
while time.time() < t_end:
# Capture current frame from the camera
ret, frame = cap.read()
# Convert the image from the camera to Gray scale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Run the detection formula
corners, ids, rP = aruco.detectMarkers(gray, aruco_dict)
# # Count the number of Arucos visible
# try:
# IDScount = len(ids)
# except:
# IDScount = 0
# Calculate the pose of the markers
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(
corners, 53, CM, dist_coef
) # <<<< IMPORTANT: number needs changing to width of printed arucos (in mm)
# Draw the detected markers as an overlay
out = aruco.drawDetectedMarkers(frame, corners, ids)
# Create Coordinate Storage Arrays
X = [] #X Coordinate Locations Array
Y = []
Z = []
ID = []
# Run loop if ids are detected
if ids is not None:
for i, id in enumerate(ids):
# Overlay the axis on the image
out = aruco.drawAxis(out, CM, dist_coef, rvecs[i][0][:],
tvecs[i][0][:], 30)
# Print the tvecs tranformation matrix or Aruco coordinates
# print("X = {:4.1f} Y = {:4.1f} Z = {:4.1f} ID = {:2d}".format(tvecs[i][0][0], tvecs[i][0][1], tvecs[i][0][2], ids[i][0]))
X.append(tvecs[i][0][0])
Y.append(tvecs[i][0][1])
Z.append(tvecs[i][0][2])
ID.append(ids[i][0])
# debugTEST = []
# Display the original frame in a window and aruco markers
cv2.imshow('frame-image', frame)
# If the button q is pressed in one of the windows
if cv2.waitKey(20) & 0xFF == ord('q'):
# Exit the While loop
break
# When everything done, release the capture
cap.release()
# close all windows
cv2.destroyAllWindows()
# # exit the kernel
# exit(0)
return X, Y, Z, ID, rvecs
def calibrate(cam, align, marker_IDs, num_markers, comm_marker_id, tf_dict,
num_pts):
"""
Args:
Returns:
"""
tolerance = 4 # degs; set by user
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
k = 0
A = []
b = []
print('Move tool to new pose, and press p to record')
while (k < num_pts):
frames = cam.pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
frame = color_image
cv2.imshow('frame', frame)
userinput = cv2.waitKey(10)
if userinput & 0xFF == ord('p'): # user wants to record point
font = cv2.FONT_HERSHEY_SIMPLEX
corners, ids, rvecs, tvecs = cam.detect_markers_realsense(frame)
if comm_marker_id in ids:
index = np.argwhere(ids == comm_marker_id)
ids = np.delete(ids, index)
shouldRecord = True
if ids is not None and len(ids) > 1:
ids.shape = (ids.size)
ids = ids.tolist()
ideal_angle = 360 / (num_markers - 1)
for i in range(len(ids) - 1):
print(ids)
marker1 = rvecs[i] # tf_dict[ids[i]]
marker1_rot, _ = cv2.Rodrigues(marker1[0])
j = i + 1
marker2 = rvecs[j] # tf_dict[ids[j]]
marker2_rot, _ = cv2.Rodrigues(marker2[0])
marker1_rot_T = marker1_rot.transpose()
rot_btw_1_2 = np.matmul(marker1_rot_T, marker2_rot)
angles = rotToEuler(rot_btw_1_2)
y_angle = np.absolute(angles[1]) * 180 / 3.1415
print(y_angle)
if (np.absolute(ids[i] - ids[j]) > 1 and \
np.absolute(ids[i] - ids[j]) < num_markers-2) or \
y_angle < ideal_angle - tolerance or \
y_angle > ideal_angle + tolerance:
shouldRecord = False
print("Bad orientation found")
break
if shouldRecord:
if comm_marker_id in ids: # if comm_marker_id detected, use point
comm_index = ids.index(comm_marker_id)
R_j, _ = cv2.Rodrigues(rvecs[comm_index])
t_j = tvecs[comm_index]
t_j.shape = (3)
else:
# transformation from marker_i frame to common_marker_id frame
R_i_comm, _ = cv2.Rodrigues(tf_dict[ids[0]][0])
t_i_comm = tf_dict[ids[0]][1]
# transformation from comm_marker_id frame to marker_i
R_comm_i = R_i_comm.transpose()
t_comm_i = np.matmul(-R_comm_i, t_i_comm)
t_comm_i.shape = (3, 1)
# transformation from marker_i to camera frame
R_i, _ = cv2.Rodrigues(rvecs[0])
t_i = tvecs[0]
t_i.shape = (3, 1)
# combine transformation tgoether to get transformation
# from comm_marker_id to camera frame
R_j = np.matmul(R_i, R_comm_i)
t_j = np.matmul(R_i, t_comm_i) + t_i
t_j.shape = (3)
# set up for pivot cal least squares equation
# A = [ R_j | -I ]
# b = [ -p_j ]
item1 = np.append(R_j[0, :], [-1, 0, 0])
item1 = item1.tolist()
A.append(item1)
item2 = np.append(R_j[1, :], [0, -1, 0])
item2 = item2.tolist()
A.append(item2)
item3 = np.append(R_j[2, :], [0, 0, -1])
item3 = item3.tolist()
A.append(item3)
b.extend(-t_j)
k = k + 1
print('Recorded')
else:
print("Not enough markers detected. Try again.")
# Solve least-squares Ax = b
A = np.asanyarray(A)
b = np.asanyarray(b)
x = np.linalg.lstsq(A, b, rcond=None)[0] # x[0:2] = p_t, x[3:5] = p_pivot
print(x[0:3])
return x[0:3]
[0, 1, 0, 1, 0, 1, 0], [0, 1, 1, 1, 1, 1, 0], [0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[16] = aruco.Dictionary_getByteListFromBits(mybits17)
#17
mybits18 = np.array(
[[0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0], [0, 1, 1, 0, 0, 1, 0],
[0, 1, 0, 1, 0, 1, 0], [0, 0, 0, 1, 1, 0, 0], [0, 1, 1, 1, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[17] = aruco.Dictionary_getByteListFromBits(mybits18)
#for i in range(len(aruco_dict.bytesList)):
# cv2.imwrite("custom_aruco_" + str(i) + ".png", aruco.drawMarker(aruco_dict, i, 128))
parameters = aruco.DetectorParameters_create() # Marker detection parameters
#markerImage = np.zeros((200, 200), dtype=np.uint8)
#markerImage = cv2.aruco.drawMarker(aruco_dict, 8, 200, markerImage, 1);
#cv2.imwrite("marker4.png", markerImage)
#cap = cv2.VideoCapture(1)
def start_node():
global detectamos, rango, direccion, profundidad, CoorMsg
rospy.init_node('camera_subscriber_hd2')
rospy.loginfo('camera_subscriber_hd2 started')
rospy.Subscriber("/zed2/left_raw/image_raw_color", Image, process_image)
rospy.Subscriber('zed2/depth_registered', Image, call_back_depth)
def detect():
if len(sys.argv) < 2:
with_feed = False
elif sys.argv[1] == '-withfeed':
with_feed = True
else:
with_feed = False
RESOLUTION = 90 # Use this to set the resolution for video feed
DEADZONE_RIGHT = 1.45
DEADZONE_LEFT = -DEADZONE_RIGHT
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, RESOLUTIONS[RESOLUTION][0])
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, RESOLUTIONS[RESOLUTION][1])
corners = np.array([[0, 0]] * 4)
# Length of the AR Tag
# In meters
marker_length = 0.06
marker_size = 0.01
ar_dict = ar.Dictionary_get(ar.DICT_6X6_250)
parameters = ar.DetectorParameters_create()
calibration_file = "calibration.xml"
calibration_params = cv2.FileStorage(calibration_file,
cv2.FILE_STORAGE_READ)
camera_matrix = calibration_params.getNode("cameraMatrix").mat()
dist_coeffs = calibration_params.getNode("distCoeffs").mat()
ret, frame = cap.read()
size = frame.shape
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]], [0, 0, 1]],
dtype='double')
while True:
ret, frame = cap.read()
size = frame.shape
picture = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected_img_points = ar.detectMarkers(
picture, ar_dict, parameters=parameters)
if with_feed:
picture = ar.drawDetectedMarkers(picture, corners)
if len(corners) == 0:
continue
# A tag is detected
print('Corners:', corners[0])
# Get rotation and translation vectors
rvec, tvec, _ = ar.estimatePoseSingleMarkers(corners[0], marker_length,
camera_matrix,
dist_coeffs)
object_x = tvec[0][0][0]
object_z = tvec[0][0][2]
print('X Distance', object_x)
print('Z Distance', object_z)
direction = get_direction(object_x, object_z)
print('Direction: ', direction)
if DEADZONE_LEFT < direction < 0:
print('Turning Left')
elif 0 < direction < DEADZONE_RIGHT:
print('Turning Right')
else:
print('Going Straight')
if with_feed:
picture = ar.drawAxis(picture, camera_matrix, dist_coeffs, rvec,
tvec, marker_size)
cv2.imshow('frame', picture)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
def __init__(self):
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
self.parameters = aruco.DetectorParameters_create()
def computeExtrinsics(imgRoot, calibDataDir, rvec=[], tvec=[], debug=False):
# Parameters rvec and tvec should give the a transformation from the world to an arbitrary reference frame
if len(rvec) == 0:
rvec = np.zeros(3).reshape(3, 1)
if len(tvec) == 0:
tvec = np.zeros(3).reshape(3, 1)
tWorld2Ref = {
"rvec": rvec,
"tvec": tvec,
"E": getRigBodyTransfMatrix(rvec, tvec)
}
## Load camera calibration
fs = cv.FileStorage(calibDataDir, cv.FILE_STORAGE_READ)
K = fs.getNode("intrinsics").mat()
distCoeffs = fs.getNode("distortion").mat()
imgSz = fs.getNode("image_size").mat()
imgSz = tuple(imgSz.reshape(1, 2)[0].astype(int))
fs.release()
## Define aruco cube coordinates
# Information given/known:
# Aruco markers dimensions = 0.69m X 0.69m
# Edge width = 0.061
# height of top face of the Aruco cube (to the floor) = 0.88
d = 0.69 / 2
e = 0.061
h = 0.88
# Transformation that converts a point in the cube reference frame to the world
tCube2World = {
"rvec": np.zeros(3).reshape(3, 1),
"tvec": np.array([[0], [0], [h - d - e]])
}
tCube2World["E"] = getRigBodyTransfMatrix(tCube2World["rvec"],
tCube2World["tvec"])
# Aruco reference frame is in the center of the cube, with:
# Z pointing upwards
# X pointing to marker 1
# Y pointing to marker 4
# Aruco corner order is clockwise.
# Aruco marker points in the world's reference frame
arucoMk0 = np.array([[d, d, h], [d, -d, h], [-d, -d, h], [-d, d, h]])
arucoMk1 = np.array([[d + e, d, h - 2 * d - e], [d + e, -d, h - 2 * d - e],
[d + e, -d, h - e], [d + e, d, h - e]])
arucoMk2 = np.array([[d, -d - e, h - e], [d, -d - e, h - 2 * d - e],
[-d, -d - e, h - 2 * d - e], [-d, -d - e, h - e]])
arucoMk3 = np.array([[], [], [], []])
arucoMk4 = np.array([[d, d + e, h - 2 * d - e], [d, d + e, h - e],
[-d, d + e, h - e], [-d, d + e, h - 2 * d - e]])
arucoMarkers = {
0: arucoMk0,
1: arucoMk1,
2: arucoMk2,
3: arucoMk3,
4: arucoMk4
}
## Apply given transformation to work the given reference frame
for key in arucoMarkers.keys():
if arucoMarkers[key].size > 0:
arucoMarkers[key] = applyRigBodyTransformation(
arucoMarkers[key], tWorld2Ref["rvec"], tWorld2Ref["tvec"])
tCube2Ref = {"E": tWorld2Ref["E"].dot(tCube2World["E"])}
tCube2Ref["rvec"], tCube2Ref["tvec"] = getRvecTvec(tCube2Ref["E"])
## Aruco library config
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
# Plot aruco marker
if debug:
sampleNum = 0
sampleMarker = aruco.drawMarker(aruco_dict, sampleNum, 500, None)
cv.imshow("Sample aruco marker nr {}".format(sampleNum), sampleMarker)
cv.imwrite("./img/sample-aruco-marker-{}.png".format(sampleNum),
sampleMarker)
cv.waitKey()
# Configuration tuning of the aruco library can yield:
mode = 2
if mode == 1:
# Good-ish results
parameters.adaptiveThreshWinSizeMin = 5
parameters.adaptiveThreshWinSizeMax = 25
parameters.adaptiveThreshWinSizeStep = 5
parameters.minMarkerPerimeterRate = 0.2
parameters.maxMarkerPerimeterRate = 4.0
parameters.minMarkerDistanceRate = 0.01
elif mode == 2:
# Great results
parameters.cornerRefinementMethod = aruco.CORNER_REFINE_APRILTAG
else:
# compared to standard values
pass
## Compute camera extrinsic calibration
tRef2Cam = {}
# For each camera
for fname in glob.glob(imgRoot + "*"):
print(fname)
# camNum indexing starts from 0
camNum = int(fname[-5]) - 1
img = cv.imread(fname)
#cv.imshow("img{}".format(camNum+1), img)
# Detect aruco markers
imgGray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
imgGray,
aruco_dict,
parameters=parameters,
cameraMatrix=K,
distCoeff=distCoeffs)
print("Detected {} markers.".format(len(ids)))
# Refine corners. Not good, worse performance
# auxCorners = np.concatenate( np.concatenate( corners, axis=0 ), axis=0 )
# criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 50, 0.01)
# auxCorners = cv.cornerSubPix(imgGray, auxCorners, winSize=(5,5), zeroZone=(-1,-1), criteria=criteria)
# drawPointsInImage("img{} : refined markers".format(camNum+1), img, auxCorners)
if debug:
# Draw detected markers
frame_markers = aruco.drawDetectedMarkers(img.copy(), corners, ids)
cv.imshow("img{} : detected markers".format(camNum + 1),
frame_markers)
frame_markers_rejected = aruco.drawDetectedMarkers(
img.copy(), rejectedImgPoints)
cv.imshow("img{} : rejected markers".format(camNum + 1),
frame_markers_rejected)
# Build correspondence between image points and object points
imagePoints = np.empty([len(ids) * 4, 2])
objectPoints = np.empty([len(ids) * 4, 3])
for i in range(len(ids)):
id = ids[i][0]
imagePoints[i * 4:(i + 1) * 4] = corners[i][0]
objectPoints[i * 4:(i + 1) * 4] = arucoMarkers[id]
# Find camera pose
flags = cv.SOLVEPNP_ITERATIVE
retval, rvec, tvec = cv.solvePnP(objectPoints,
imagePoints,
cameraMatrix=K,
distCoeffs=distCoeffs,
flags=flags)
tRef2Cam[camNum] = {
"rvec": rvec,
"tvec": tvec,
"E": getRigBodyTransfMatrix(rvec, tvec)
}
# Refine?
#solvePnPRefineLM()
#solvePnPRefineVVS()
if debug:
# Project 3D points back to the camera with the estimated camera pose
imagePointsEst, _ = cv.projectPoints(objectPoints,
tRef2Cam[camNum]["rvec"],
tRef2Cam[camNum]["tvec"],
cameraMatrix=K,
distCoeffs=distCoeffs)
imagePointsEst = imagePointsEst.round()
annotatedImg = drawPointsInImage(img.copy(), imagePointsEst)
# Draw cube reference frame
rvec, tvec = getRvecTvec(tRef2Cam[camNum]["E"].dot(tCube2Ref["E"]))
annotatedImg = addReferenceMarker(annotatedImg, rvec, tvec, K,
distCoeffs, d + e)
cv.imshow(
"Aruco edge points projected back to cam{} with the estimated pose and cube reference frame"
.format(camNum + 1), annotatedImg)
cv.waitKey()
## Compute transformation between cameras
transfI2J = []
for i in range(len(tRef2Cam)):
auxt = []
for j in range(len(tRef2Cam)):
# Shown 2 alternatives to report the transformations between camera reference frames
# Option 1: Euclidean rigid body transformation matrix (it's simpler to write and read)
# Get Euclidean rigid body transformation
Ei = tRef2Cam[i]["E"]
Ej = tRef2Cam[j]["E"]
# Find the transformation that converts a point in camera i reference frame back to the world
retval, invEi = cv.invert(Ei)
if not retval == 1:
print(
"Failed to invert camera {} extrinsic calibration".format(
i))
exit(-1)
# Compose both transformations to get a transformation from reference i to j
dic = {"E": Ej.dot(invEi)}
# Option 2: rotation matrix/vector and translation vector
Ri, _ = cv.Rodrigues(tRef2Cam[i]["rvec"])
Ti = tRef2Cam[i]["tvec"]
Rj, _ = cv.Rodrigues(tRef2Cam[j]["rvec"])
Tj = tRef2Cam[j]["tvec"]
# Find the transformation that converts a point in camera i reference frame in camera j
retval, invRi = cv.invert(Ri)
if not retval == 1:
print("Failed to invert camera {} rotation matrix".format(i))
exit(-1)
dic["rvec"] = cv.Rodrigues(Rj.dot(invRi))[0]
dic["tvec"] = -Rj.dot(invRi).dot(Ti) + Tj
if debug:
# Checking if reported transforms are equivalent
print(
np.sum(dic["E"] -
getRigBodyTransfMatrix(dic["rvec"], dic["tvec"])))
auxt.append(dic)
transfI2J.append(auxt)
if debug:
## Quick tests
# Sanity check. Transformation from camera i to j is the inverse of the one from camera j to i
for i in range(len(transfI2J)):
for j in range(i, len(transfI2J)):
print(
np.all(
transfI2J[i][j]["E"].dot(transfI2J[j][i]["E"]) -
np.eye(4) < np.finfo(transfI2J[i][j]["E"].dtype).eps *
10))
# Test: Plot camera reference frames. Not working! Needs specific code to deal with points outside camera FOV
# Standard opencv function does not deal with this well
# annotatedImg2 = img.copy()
# for i in range(len(transfI2J)):
# rvec, tvec = getRvecTvec( transfI2J[i][camNum] )
# annotatedImg = addReferenceMarker(annotatedImg2, rvec, tvec, K, distCoeffs, 4)
# cv.imshow("annotated img with camera reference frames", annotatedImg2)
cv.waitKey()
return tRef2Cam, transfI2J
## Might come in handy
# sift = cv.SIFT_create()
# kp = sift.detect(imgGray,None)
# img=cv.drawKeypoints(imgGray,kp,img)
# cv.imshow("sift", img)
#
# cv.waitKey()
def aruco_coordinates(file_name):
ids = None
flag_contour = 0
cap = cv2.VideoCapture(file_name) # Capture video from camera
while (cap.isOpened() and (flag_contour == 0 or ids == None)):
ret, frame = cap.read()
area = (frame.shape)
frame_area = area[0] * area[1]
print("Frame Area")
#print(frame_area)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if (ids == None):
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
gray = clahe.apply(gray)
gray_aruco = 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, aruco_dict, parameters=parameters)
print("id of aruco = ")
print(ids.flatten())
if flag_contour == 0:
#contour filtering part
blurred = cv2.bilateralFilter(gray, 11, 17, 17)
kernel = np.ones((5, 5), np.uint8)
blurredopen = cv2.morphologyEx(blurred, cv2.MORPH_OPEN, kernel)
blurredopen = cv2.morphologyEx(blurredopen, cv2.MORPH_OPEN, kernel)
blurredclose = cv2.morphologyEx(blurredopen, cv2.MORPH_CLOSE,
kernel)
edged = cv2.Canny(blurredclose, 30, 200)
cnts = cv2.findContours(edged.copy(), cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# cnts = sorted(cnts, key = cv2.contourArea, reverse = True)[:10]
cnts = cnts[0] if imutils.is_cv2() else cnts[1]
cntsSorted = sorted(cnts,
key=lambda x: cv2.contourArea(x),
reverse=True)[:1]
for c in cntsSorted:
# approximate the contour
peri = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, 0.01 * peri, True)
# if our approximated contour has four points, then
# we can assume that we have found our screen
if len(approx) == 4:
contour_area = (cv2.contourArea(c))
#print("Area Percent")
# print(cv2.contourArea(c))
areapercent = (contour_area / frame_area) * 100
#print(areapercent)
if areapercent > 25:
#print("Arena Found")
screenCnt = approx
contours = screenCnt
flag_contour = 1
if flag_contour == 1 and ids != None:
break
print("i am out")
cap.release()
return ((ids.flatten()), contours)
def detect_markers_and_draw(mtx, dist, frame, drawing_img):
marker_size = 2.8/100 # marker length in meters
axis_length = 0.015
# get dictionary and parameters
parameters = aruco.DetectorParameters_create() # create parameters
parameters.detectInvertedMarker = False # enable detection of inverted markers
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # convert to gray
# get marker data
corners, ids, rejected_img_points = aruco.detectMarkers(gray, ARUCO_DICT, parameters=parameters)
# draw corners on frame
frame_m = aruco.drawDetectedMarkers(frame, corners, ids)
if len(corners) == 4:
# get corners of initial image
y_max = frame.shape[0]
x_max = frame.shape[1]
frame_corners = [[0, 0], [x_max, 0], [0, y_max], [x_max, y_max]]
# get transform corners
transform_corners = np.zeros((len(ids), 2))
for i in range(len(ids)):
corner_num = ids[i][0]
# get center x and y (calculating average)
x, y = 0, 0
for j in range(4):
x += corners[i][0][j][0]
y += corners[i][0][j][1]
# add center to transform matrix
transform_corners[corner_num] = [x/4, y/4]
# transform drawing
pts1 = np.float32(frame_corners)
pts2 = np.float32(transform_corners)
M = cv2.getPerspectiveTransform(pts1, pts2)
drawing_img = cv2.warpPerspective(drawing_img, M, (drawing_img.shape[1], drawing_img.shape[0]))
# create mask
gray_square = cv2.cvtColor(drawing_img, cv2.COLOR_BGR2GRAY)
ret, mask = cv2.threshold(gray_square, 5, 255, cv2.THRESH_BINARY)
mask_inv = cv2.bitwise_not(mask)
# get background from frame
# print(f'frame_m shape {frame_m.shape}')
# print(f'mask shape {mask_inv.shape}')
frame_bg = cv2.bitwise_and(frame_m, frame_m, mask=mask_inv)
# take only drawing from drawing image
drawing_img = cv2.bitwise_and(drawing_img, drawing_img, mask=mask)
frame_m = cv2.add(frame_bg, drawing_img)
# point_1 = (int(corners[0][0][0][0]), (corners[0][0][0][1]))
# point_2 = (int(corners[1][0][0][0]), (corners[1][0][0][1]))
# cv2.line(square_img, point_1, point_2, (255, 0, 0), 3)
# estimate pose
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, marker_size, mtx, dist)
if tvecs is not None:
for i in range(len(tvecs)):
frame_m = aruco.drawAxis(frame_m, mtx, dist, rvecs[i], tvecs[i], axis_length)
return frame_m, rvecs, tvecs
def show_webcam(mirror=False):
cam = cv2.VideoCapture(0) # Capture Webcam
BLUE = (255, 0, 0)
KNOWN_WIDTH_INCHES = 4.55 # AR Marker width in inches
KNOWN_WIDTH_MM = KNOWN_WIDTH_INCHES * 25.4
WIDTH = 480 # Image width in pixels
HEIGHT = 640 # image height in pixels
PIXEL_SIZE = 310 # Size of AR Marker in Pixels
DISTANCE0 = 12 # 1 foot in inches
FOCAL_LENGTH = DISTANCE0 * PIXEL_SIZE / KNOWN_WIDTH_INCHES
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
font = cv2.FONT_HERSHEY_PLAIN
while True:
ret_val, frame = cam.read() # Read each frame from camera
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Gray scale image
red_img = frame.copy()
# set blue and green channels to 0
red_img[:, :, 0] = 0
red_img[:, :, 1] = 0
#Look for faces
faces = face_cascade.detectMultiScale(gray, 1.1, 4)
ar_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
param = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(frame,
ar_dict,
parameters=param)
ar_gray = aruco.drawDetectedMarkers(frame, corners)
if (len(corners) > 0):
Xcord = [corners[0][0][0][0], corners[0][0][2][0]]
Ycord = [corners[0][0][0][1], corners[0][0][2][1]]
pixel_size = corners[0][0][0][0] - corners[0][0][1][0]
distance = FOCAL_LENGTH * KNOWN_WIDTH_INCHES / pixel_size
print("Distance to Object " + str(distance) + "\n")
x = midPointCalculation(Xcord)
y = midPointCalculation(Ycord)
center = (int(x), int(y))
#print(str(x) +","+ str(y))
LEFT_BOUND = 215
RIGHT_BOUND = 430
if x < LEFT_BOUND:
pass
elif x > RIGHT_BOUND:
pass
else:
pass
print("\n")
cv2.circle(gray, center, 5, BLUE, thickness=-1)
#Look for faces
if len(faces) != 0:
cv2.putText(red_img, "FACE DETECTED", (250, 435), font, 2,
(255, 255, 255))
for (x, y, w, h) in faces:
cv2.rectangle(red_img, (x, y), (x + w, y + h), (255, 0, 0), 2)
cv2.imshow("Vision", gray)
if cv2.waitKey(1) == 27:
break # esc to quit
cam.release() # Release capture
cv2.destroyAllWindows() # Close windows
def main():
face_landmark_path = './shape_predictor_68_face_landmarks.dat'
#flags/initializations to be set
skip_frame = 3
socket_connect = 1 # set to 0 if we are testing the code locally on the computer with only the realsense tracking.
simplified_calib_flag = 0 # this is set to 1 if we want to do one-time calibration
arucoposeflag = 1
# img_idx keeps track of image index (frame #).
img_idx = 0
N_samples_calib = 10 # number of samples for computing the calibration matrix using homography
if socket_connect == 1:
# create a socket object
s = socket.socket()
print("Socket successfully created")
# reserve a port on your computer in our case it is 2020 but it can be anything
port = 2020
s.bind(('', port))
print("socket binded to %s" % (port))
# put the socket into listening mode
s.listen(5)
print("socket is listening")
c, addr = s.accept()
print('got connection from ', addr)
if socket_connect == 1 and simplified_calib_flag == 0:
arucoinstance = arucocalibclass()
ReturnFlag = 1
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
marker_len = 0.0645
MLRSTr = arucoinstance.startcamerastreaming(c, ReturnFlag, marker_len,
aruco_dict,
N_samples_calib)
print(MLRSTr)
elif socket_connect == 1 and simplified_calib_flag == 1:
T_M2_RS = np.array([
-1.0001641, 0.00756584, 0.00479072, 0.03984956, -0.00774137,
-0.99988126, -0.03246199, -0.01359556, 0.00453644, -0.03251681,
0.99971441, -0.00428408, 0., 0., 0., 1.
])
T_M2_RS = T_M2_RS.reshape(4, 4)
MLRSTr = simplified_calib(T_M2_RS, c)
else:
MLRSTr = np.array((1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1))
MLRSTr = MLRSTr.reshape(4, 4)
print(MLRSTr)
# end socket ######
# Configure depth and color streams of Realsense camera
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
# Intrinsics & Extrinsics of Realsense
depth_intrin = profile.get_stream(
rs.stream.depth).as_video_stream_profile().get_intrinsics()
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]])
dist = np.array(intr.coeffs)
#load cascade classifier training file for lbpcascade
lbp_face_cascade = cv2.CascadeClassifier(
"./haarcascade_frontalface_alt2.xml"
) #"/home/supriya/.local/lib/python3.6/site-packages/cv2/data/haarcascade_frontalface_alt2.xml") #cv2.CascadeClassifier('/home/supriya/supriya/FSA-Net/demo/lbpcascade_frontalface.xml') #cv2.CascadeClassifier('data/lbpcascade_frontalface_improved.xml') # cv2.CascadeClassifier('/home/supriya/supriya/FSA-Net/demo/lbpcascade_frontalface.xml')
facemark = cv2.face.createFacemarkLBF()
facemark.loadModel('./lbfmodel.yaml')
print('Start detecting pose ...')
detected_pre = []
while True:
# get video frame
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()
aligned_depth_frame = aligned_frames.get_depth_frame()
if not aligned_depth_frame or not color_frame:
continue
# read color frame
input_img = np.asanyarray(color_frame.get_data())
#increment count of the image index
img_idx = img_idx + 1
img_h, img_w, _ = np.shape(input_img)
# Process the first frame and every frame after "skip_frame" frames
if img_idx == 1 or img_idx % skip_frame == 0:
# convert image to grayscale
gray_img = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# detect faces using LBP detector
faces = lbp_face_cascade.detectMultiScale(gray_img,
scaleFactor=1.1,
minNeighbors=5)
#draw rectangle around detected face
for (x, y, w, h) in faces:
cv2.rectangle(input_img, (x, y), (x + w, y + h), (0, 255, 0),
2)
depth_point = [0, 0, 0]
if len(faces) > 0:
#detect landmarks
status, landmarks = facemark.fit(gray_img, faces)
#draw dots on the detected facial landmarks
for f in range(len(landmarks)):
cv2.face.drawFacemarks(input_img, landmarks[f])
#get head pose
reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts = get_head_pose(
landmarks[0][0], mtx, dist)
# draw circle on image points (nose tip, corner of eye, lip and chin)
for p in image_pts:
cv2.circle(input_img, (int(p[0]), int(p[1])), 3,
(0, 0, 255), -1)
# draw circle on image points (nose tip, corner of eye, lip and chin)
for p in image_pts:
cv2.circle(input_img, (int(p[0]), int(p[1])), 3,
(0, 0, 255), -1)
# draw line sticking out of the nose tip and showing the head pose
(nose_end_point2D,
jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotation_vec, translation_vec,
mtx, dist)
p1 = (int(image_pts[0][0]), int(image_pts[0][1]))
p2 = (int(nose_end_point2D[0][0][0]),
int(nose_end_point2D[0][0][1]))
cv2.line(input_img, p1, p2, (255, 0, 0), 2)
# get 3D co-ord of nose - to get a more accurate estimation of the translaion of the head
depth = aligned_depth_frame.get_distance(
landmarks[0][0][30][0], landmarks[0][0][30][1])
cv2.circle(input_img,
(landmarks[0][0][30][0], landmarks[0][0][30][1]), 3,
(0, 255, 0), -1)
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin,
[landmarks[0][0][30][0], landmarks[0][0][30][1]], depth)
depth_point = np.array(depth_point)
depth_point = np.reshape(depth_point, [1, 3])
#check if the depth estimation is not zero and filters out faces within 0.8 m from the camera
if (depth_point[0][2] != 0 and depth_point[0][2] < 0.8): #
#Combine rotation matrix and translation vector (given by the depth point) to get the head pose
RSTr = np.hstack([rotation_mat, depth_point.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
print("head pose", RSTr)
# If arucoposeflag = 1, an Aruco marker will get detected and its transformed pose will be streamed to the AR headset and the pose
# of the tracking parent will be updated to reflect Aruco marker pose. This can be used to verify/test the accuracy of teh calibration
if arucoposeflag == 1:
print("aruco")
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# set dictionary size depending on the aruco marker selected
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoindetectorts = 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 np.all(ids != None):
# estimate pose of each marker and return the values
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, 0.045, mtx, dist
) # 0.0628 (0.061 if using Dell laptop - 95% zoom)
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(input_img, mtx, dist, rvec[i],
tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(input_img, corners)
#Combine rotation matrix and translation vector to get Aruco pose
R_rvec = R.from_rotvec(rvec[0])
R_rotmat = R_rvec.as_matrix()
AruRSTr = np.hstack(
[R_rotmat[0], tvec[0].transpose()])
AruRSTr = np.vstack([AruRSTr, [0, 0, 0, 1]])
RSTr = AruRSTr
print("Aruco pose", AruRSTr)
# Since pose detected in OpenCV will be right handed coordinate system, it needs to be converted to left-handed coordinate system of Unity
RSTr_LH = np.array([
RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3],
RSTr[2][0], RSTr[2][2], RSTr[2][1], RSTr[2][3],
RSTr[1][0], RSTr[1][2], RSTr[1][1], RSTr[1][3],
RSTr[3][0], RSTr[3][1], RSTr[3][2], RSTr[3][3]
]) # converting to left handed coordinate system
RSTr_LH = RSTr_LH.reshape(4, 4)
#Compute the transformed pose to be streamed to MagicLeap
HeadPoseTr = np.matmul(MLRSTr, RSTr_LH)
if socket_connect == 1:
# Array to be sent
ArrToSend = np.array([
HeadPoseTr[0][0], HeadPoseTr[0][1],
HeadPoseTr[0][2], HeadPoseTr[0][3],
HeadPoseTr[1][0], HeadPoseTr[1][1],
HeadPoseTr[1][2], HeadPoseTr[1][3],
HeadPoseTr[2][0], HeadPoseTr[2][1],
HeadPoseTr[2][2], HeadPoseTr[2][3],
HeadPoseTr[3][0], HeadPoseTr[3][1],
HeadPoseTr[3][2], HeadPoseTr[3][3]
])
# Send transformed pose to AR headset
dataTosend = struct.pack('f' * len(ArrToSend),
*ArrToSend)
c.send(dataTosend)
else:
print("No Face Detected")
# display detected face with landmarks, pose.
cv2.imshow('Landmark_Window', input_img)
key = cv2.waitKey(1)
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,
ugly_const.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,
ugly_const.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 __init__(self):
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
self.params = aruco.DetectorParameters_create()
self.cap = cv2.VideoCapture(0)
self.has_screen = bool(os.environ.get('DISPLAY', None))
def Follow():
global CornerCounter
cap = cv2.VideoCapture(0)
print(cap.isOpened())
time.sleep(1)
_, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, 50, 150, apertureSize=3)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
for i in range(4):
_, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, 50, 150, apertureSize=3)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
for x in range(0, len(ids)):
Tempx = corners[x][0][0][0] + corners[x][0][1][0] + corners[x][0][
2][0] + corners[x][0][3][0]
Tempx = Tempx / 4
Tempy = corners[x][0][0][1] + corners[x][0][1][1] + corners[x][0][
2][1] + corners[x][0][3][1]
Tempy = Tempy / 4
if ids[x] == CVConstants.corner_ids[0]:
globals.CornerPositions[0] = [Tempx, Tempy]
elif ids[x] == CVConstants.corner_ids[1]:
globals.CornerPositions[1] = [Tempx, Tempy]
elif ids[x] == CVConstants.corner_ids[2]:
globals.CornerPositions[2] = [Tempx, Tempy]
elif ids[x] == CVConstants.corner_ids[3]:
globals.CornerPositions[3] = [Tempx, Tempy]
#get all vertices:
lowest_sum = globals.CornerPositions[0][0] + globals.CornerPositions[0][1]
highest_sum = lowest_sum
bottom_right = globals.CornerPositions[0]
bottom_left = globals.CornerPositions[0]
top_right = globals.CornerPositions[0]
top_left = globals.CornerPositions[0]
taken = [0, 0]
for i in range(4):
cx, cy = globals.CornerPositions[i]
sum = cx + cy
if sum < lowest_sum:
lowest_sum = sum
bottom_right = globals.CornerPositions[i]
taken[0] = i
elif sum > highest_sum:
highest_sum = sum
top_left = globals.CornerPositions[i]
taken[1] = i
for i in range(4):
if i not in taken:
bottom_left = globals.CornerPositions[i]
taken.append(i)
for i in range(4):
if i not in taken:
top_right = globals.CornerPositions[i]
taken.append(i)
if bottom_left[0] > top_right[0]:
bottom_left, top_right = top_right, bottom_left
N = len(globals.Grid)
hx = (top_left[0] - bottom_right[0]) / (N - 1)
hy = (top_left[1] - bottom_right[1]) / (N - 1)
for i in range(N):
for j in range(N):
globals.Grid[i][j][0] = bottom_right[0] + hx * i
globals.Grid[i][j][1] = bottom_right[1] + hy * j
while (1):
_, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, 50, 150, apertureSize=3)
font = cv2.FONT_HERSHEY_SIMPLEX
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
CornerPositions = globals.CornerPositions
cv2.circle(img,
(int(CornerPositions[0][0]), int(CornerPositions[0][1])),
20, (0, 255, 0), -1)
cv2.circle(img,
(int(CornerPositions[1][0]), int(CornerPositions[1][1])),
20, (0, 255, 0), -1)
cv2.circle(img,
(int(CornerPositions[2][0]), int(CornerPositions[2][1])),
20, (0, 250, 0), -1)
cv2.circle(img,
(int(CornerPositions[3][0]), int(CornerPositions[3][1])),
20, (0, 0, 255), -1)
cv2.line(img, (int(CornerPositions[0][0]), int(CornerPositions[0][1])),
(int(CornerPositions[1][0]), int(CornerPositions[1][1])),
(255, 0, 0), 2)
cv2.line(img, (int(CornerPositions[0][0]), int(CornerPositions[0][1])),
(int(CornerPositions[3][0]), int(CornerPositions[3][1])),
(255, 0, 0), 2)
cv2.line(img, (int(CornerPositions[3][0]), int(CornerPositions[3][1])),
(int(CornerPositions[2][0]), int(CornerPositions[2][1])),
(255, 0, 0), 2)
cv2.line(img, (int(CornerPositions[2][0]), int(CornerPositions[2][1])),
(int(CornerPositions[1][0]), int(CornerPositions[1][1])),
(255, 0, 0), 2)
if not corners:
pass
else:
for x in range(0, len(ids)):
if (len(corners[x][0]) < 4):
print("No 4 corners?")
print(corners[x][0])
if ids[x] in CVConstants.bot_ids:
idd = ids[x]
index = -1
for i in range(len(CVConstants.bot_ids)):
if idd == CVConstants.bot_ids[i]:
index = i
break
if index == -1:
continue
globals.RobotState[index][0] = (
corners[x][0][0][0] + corners[x][0][1][0] +
corners[x][0][2][0] + corners[x][0][3][0]) / 4
globals.RobotState[index][1] = (
corners[x][0][0][1] + corners[x][0][1][1] +
corners[x][0][2][1] + corners[x][0][3][1]) / 4
cv2.line(
img,
(int(corners[x][0][2][0]), int(corners[x][0][2][1])),
(int(corners[x][0][1][0]), int(corners[x][0][1][1])),
(65, 255, 32), 2)
heading = getAngleBetweenPoints(int(corners[x][0][2][0]),
int(corners[x][0][2][1]),
int(corners[x][0][1][0]),
int(corners[x][0][1][1]))
heading_degrees = math.degrees(heading)
globals.RobotState[index][2] = heading_degrees
cv2.putText(img, str(heading_degrees), (100, 200), font,
0.5, (0, 255, 0), 2, cv2.LINE_AA)
gray = aruco.drawDetectedMarkers(img, corners, ids, (0, 255, 255))
cv2.imshow('img', img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
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()
cap = cv2.VideoCapture(0)
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640);
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480);
camera_matrix = np.array([[1776, 0, 762], [0, 1780, 1025], [0, 0, 1]],
dtype=float)
dist_coeffs = np.array([[0, 0, 0, 0]], dtype=float)
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_6X6_50)
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)
#It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
colored = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
def aruco_fun():
global pipe, cameraMatrix, distCoeffs, depth_intrinsics
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
if True:
frames = pipe.wait_for_frames()
align_to = rs.stream.color
align = rs.align(align_to)
aligned_frames = align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
color_img = np.array(color_frame.get_data())
color_img_temp = copy.deepcopy(color_img)
depth_img = np.array(depth_frame.get_data())
frame = color_img_temp
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)
#print(corners,ids)
#center_p(corners, depth_img,cameraMatrix)
#if False:
if ids is not None and len(ids) > 0:
global wheel_chair_translation_vectors, find_wheel_chair
#print("len",len(ids[0]))
num = int(len(ids))
for id_obj in range(num):
if ids[id_obj] == 6:
find_wheel_chair = 1
wheel_chair_translation_vectors = get_xyz(corners[id_obj], depth_img,cameraMatrix,depth_intrinsics)
elif ids[id_obj]!=6:
if find_wheel_chair == 0:
continue
obj_translation_vectors = get_xyz(corners[id_obj], depth_img,cameraMatrix,depth_intrinsics)
p = obj_translation_vectors - wheel_chair_translation_vectors
print(ids[id_obj]," ","postion vetor is",p)
#process_data(ids[id_obj],p)
gray = aruco.drawDetectedMarkers(gray, corners)
cv2.imshow('frame',gray)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
#break
if False:
#while(True):
frames = pipe.wait_for_frames()
color_frame = frames.get_color_frame()
# Convert images to numpy arrays
color_image = np.asanyarray(color_frame.get_data())
gray = cv2.cvtColor(color_image.copy(), cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image = gray,
board = board,
detectedCorners = corners,
detectedIds = ids,
rejectedCorners = rejectedImgPoints,
cameraMatrix = cameraMatrix,
distCoeffs = distCoeffs)
ProjectImage = color_image.copy()
ProjectImage = aruco.drawDetectedMarkers(ProjectImage, corners, borderColor=(0, 0, 255))
print(ids)
if ids is not None and len(ids) > 0:
# Estimate the posture per each Aruco marker
rotation_vectors, translation_vectors, _objPoints = aruco.estimatePoseSingleMarkers(corners, 1, cameraMatrix, distCoeffs)
#global wheel_chair_rotation_vectors, wheel_chair_translation_vectors, find_wheel_chair ,w2c
print("len",len(ids[0]))
num = int(len(ids))
for id_obj in range(num):
if ids[id_obj] == 6:
find_wheel_chair = 1
wheel_chair_rotation_vectors =cv2.Rodrigues( rotation_vectors[id_obj][0])[0]
wheel_chair_translation_vectors = rotation_vectors[id_obj]
c2w = np.c_[wheel_chair_rotation_vectors,wheel_chair_translation_vectors[0]]
temp = np.array([0,0,0,1])
c2w = np.r_[c2w, [temp]]
#w2c = np.linalg.inv(c2w)
w2c = np.c_[wheel_chair_rotation_vectors.T,-wheel_chair_rotation_vectors.T.dot(wheel_chair_translation_vectors[0])]
w2c = np.r_[w2c, [temp]]
elif ids[id_obj]!=6:
if find_wheel_chair == 0:
continue
obj_rotation_vectors =cv2.Rodrigues( rotation_vectors[id_obj][0])[0]
obj_translation_vectors = rotation_vectors[id_obj]
c2o = np.c_[obj_rotation_vectors,obj_translation_vectors[0]]
temp = np.array([0,0,0,1])
c2o = np.r_[c2o, [temp]]
w2o = w2c.dot(c2o)
#p = w2o[:,3][:3]
p = compute_w2o(wheel_chair_rotation_vectors,wheel_chair_translation_vectors,obj_rotation_vectors,obj_translation_vectors)
print(ids[id_obj]," ","postion vetor is",p)
#process_data(ids[id_obj],p)
ids = 1
for rvec in rotation_vectors:
rotation_mat = cv2.Rodrigues(rvec[0])[0]
ids = ids+1
for rvec, tvec in zip(rotation_vectors, translation_vectors):
if len(sys.argv) == 2 and sys.argv[1] == 'cube':
try:
imgpts, jac = cv2.projectPoints(axis, rvec, tvec, cameraMatrix, distCoeffs)
ProjectImage = drawCube(ProjectImage, corners, imgpts)
except:
continue
else:
ProjectImage = aruco.drawAxis(ProjectImage, cameraMatrix, distCoeffs, rvec, tvec, 1)
cv2.imshow('ProjectImage', ProjectImage)
#frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
#cv2.imshow('frame',frame_markers)
#end = datetime.now()
#exec_time = end - start
#print("aruco control")
#print(exec_time,exec_time.total_seconds())
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
