cam.set(cv2.CAP_PROP_FPS, FPS)
arudict = aruco.custom_dictionary(8, 3)
logitech_cam_matrix = np.array([6.6419013281274511e+02, 0., 3.3006505461506646e+02, 0.,
6.6587003740196826e+02, 2.3030870519881907e+02, 0., 0., 1.])
logitech_cam_matrix = logitech_cam_matrix.reshape((3, 3))
logitech_dist_coeffs = np.array([2.0806786957819065e-01, -3.6839605435678208e-01,
-1.2078578161331370e-02, 6.4294407773653481e-03,
4.2723310854154123e-01])
car_board0 = aruco.Board_create(objPoints, arudict, np.arange(0, 4))
car_board1 = aruco.Board_create(objPoints, arudict, np.arange(4, 8))
while True:
ret, frame = cam.read()
corners, ids, rejected = aruco.detectMarkers(frame, arudict)
corners, ids, rejected, recovered = aruco.refineDetectedMarkers(frame, car_board0, corners, ids, rejected,
logitech_cam_matrix, logitech_dist_coeffs)
corners, ids, rejected, recovered = aruco.refineDetectedMarkers(frame, car_board1, corners, ids, rejected,
logitech_cam_matrix, logitech_dist_coeffs)
aruco.drawDetectedMarkers(frame, corners, ids)
# rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, sd, camera_matrix, dist_coeffs)
N0, rvec0, tvec0 = aruco.estimatePoseBoard(corners, ids, car_board0, logitech_cam_matrix, logitech_dist_coeffs)
N1, rvec1, tvec1 = aruco.estimatePoseBoard(corners, ids, car_board1, logitech_cam_matrix, logitech_dist_coeffs)
if N0:
# for i in range(len(ids)):
# aruco.drawAxis(frame, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.4)
mat, jacob = cv2.Rodrigues(rvec0)
dash = np.full((400, 600), 255, dtype=np.uint8)
putTextMultiline(dash, repr(mat), (20, 20))
putTextMultiline(dash, repr(tvec0), (400, 200))
putTextMultiline(dash, repr(rvec0), (20, 200))
def detection_markers():
"""
"""
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_1000)
pckl_file_exist = False
if not os.path.exists('calibration.pckl'):
print("You need to calibrate the camera you'll be using. See \
calibration project directory for details.")
exit()
else:
pckl_file_exist = True
f = open('calibration.pckl', 'rb')
(cameraMatrix, distCoeffs) = pickle.load(f)
f.close()
if cameraMatrix is None or distCoeffs is None:
print(
"Calibration issue. Remove ./calibration.pckl and recalibrate\
your camera with CalibrateCamera.py.")
exit()
# Create grid board object we're using in our stream
board = aruco.GridBoard_create(markersX=5,
markersY=7,
markerLength=0.04,
markerSeparation=0.01,
dictionary=ARUCO_DICT)
# Create vectors we'll be using for rotations and translations for postures
rvecs, tvecs = None, None
cam = cv2.VideoCapture(0)
while (cam.isOpened() and pckl_file_exist):
# Exit at the end of the video on the 'q' keypress
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Capturing each frame of our video stream
ret, img = cam.read()
if ret == True:
# grayscale image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refind not detected markers based on the already detected and
# the board layout.
corners, ids, _, _ = aruco.refineDetectedMarkers(
image=gray,
board=board,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
side_len = 0.5
axis = np.float32([[-side_len, -side_len, 0],
[-side_len, side_len,
0], [side_len, side_len, 0],
[side_len, -side_len, 0],
[-side_len, -side_len, 1],
[-side_len, side_len,
1], [side_len, side_len, 1],
[side_len, -side_len, 1]])
# Draw cube only when there is only one marker in a frame
if ids is not None and len(ids) == 1:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, ids, cameraMatrix, distCoeffs)
# The function computes projections of 3D points to the image
# plane given intrinsic and extrinsic camera parameters.
# imgpts - output array of image points in camera coordinate
# system (x and y)
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, cameraMatrix,
distCoeffs)
# Display image with a cube
draw(img, imgpts, corners)
cv2.waitKey(2)
cv2.destroyAllWindows()
def run(self):
# Initialise subscribing topics
self.init_ros_topics()
# Wait for cameras to be ready before going ahead
while ((self.cam1_state != "ready") and (not rospy.is_shutdown())): # or (self.cam2_state != "ready")):
continue
if rospy.is_shutdown():
return
# load camera calibration info from cameras
calib = rospy.wait_for_message('/camera/color/camera_info', CameraInfo)
# calib2 = rospy.wait_for_message('/camera2/color/camera_info', CameraInfo)
mtx = np.reshape(calib.K, [3,3])
dist = np.array([calib.D])
# marker size in meters.
board_square_length = 0.032 # 0.042
board_marker_size = 0.0265 # 0.034
robot_square_length = 0.161
robot_marker_size = 0.140
robot_left_square_length = 0.134
robot_left_marker_size = 0.104
watch_square_length = 0.07 #0.0595
watch_marker_size = 0.0535#0.0455#0.043
# robot on the right
# initialize array to store location data for base markers in the camera's frame.
base_marker1 = np.array([0,0,0])
base_marker2 = np.array([0,0,0])
base_marker3 = np.array([0,0,0])
# robot on the left
#initialize location data for base markers in the camera's frame.
base_marker_l1 = np.array([0,0,0])
base_marker_l2 = np.array([0,0,0])
base_marker_l3 = np.array([0,0,0])
# watch
#initialize location data for base markers in the camera's frame.
watch_base_marker1 = np.array([0,0,0])
watch_base_marker2 = np.array([0,0,0])
watch_base_marker3 = np.array([0,0,0])
watch_base_marker4 = np.array([0,0,0])
# initilaize location data for charuco board
board_origin = np.array([0,0,0])
# defining different dictionaries to use for different coordinate systems
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
aruco_dict_robot_l = aruco.Dictionary_get(aruco.DICT_6X6_250)
aruco_dict_robot_r = aruco.Dictionary_get(aruco.DICT_4X4_250)
aruco_dict_watch = aruco.Dictionary_get(aruco.DICT_7X7_250)
board = aruco.CharucoBoard_create(9, 6, board_square_length, board_marker_size, aruco_dict)
charuco_robot_r = aruco.CharucoBoard_create(3, 3, robot_square_length, robot_marker_size, aruco_dict_robot_r)
charuco_robot_l = aruco.CharucoBoard_create(3, 3, robot_left_square_length, robot_left_marker_size, aruco_dict_robot_l)
charuco_watch = aruco.CharucoBoard_create(3, 3, watch_square_length, watch_marker_size, aruco_dict_watch)
parameters = aruco.DetectorParameters_create()
# filter
a_filter = 0.95
# Initialization of variables
Tb_r = np.identity(4)
rot_mat = np.identity(3)
robot_origin = np.array([[0, 0, 0]])
Tb_l = np.identity(4)
rot_mat_l = np.identity(3)
robot_origin_l = np.array([[0, 0, 0]])
Tb_w = np.identity(4)
rot_mat_w = np.identity(3)
watch_origin = np.array([[0, 0, 0]])
rate = rospy.Rate(30)
while not rospy.is_shutdown():
QueryImg = copy.deepcopy(self.cam1['rgb'])
# cv2.imwrite('./out/imgA_{}.png'.format(self.fr1), QueryImg)
# grayscale image
gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers, _r refers to robot on the right, _l referes to robot on the left.
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
corners_r, ids_r, rejectedImgPoints_r = aruco.detectMarkers(gray, aruco_dict_robot_r, parameters=parameters)
corners_l, ids_l, rejectedImgPoints_l = aruco.detectMarkers(gray, aruco_dict_robot_l, parameters=parameters)
watch_corners, watch_ids, watch_rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict_watch, parameters=parameters)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image = gray,
board = board,
detectedCorners = corners,
detectedIds = ids,
rejectedCorners = rejectedImgPoints,
cameraMatrix = mtx,
distCoeffs = dist)
corners_r, ids_r, rejectedImgPoints_r, recoveredIds_r = aruco.refineDetectedMarkers(
image = gray,
board = charuco_robot_r,
detectedCorners = corners_r,
detectedIds = ids_r,
rejectedCorners = rejectedImgPoints_r,
cameraMatrix = mtx,
distCoeffs = dist)
corners_l, ids_l, rejectedImgPoints_l, recoveredIds_l = aruco.refineDetectedMarkers(
image = gray,
board = charuco_robot_l,
detectedCorners = corners_l,
detectedIds = ids_l,
rejectedCorners = rejectedImgPoints_l,
cameraMatrix = mtx,
distCoeffs = dist)
watch_corners, watch_ids, watch_rejectedImgPoints, watch_recoveredIds = aruco.refineDetectedMarkers(
image = gray,
board = charuco_watch,
detectedCorners = watch_corners,
detectedIds = watch_ids,
rejectedCorners = watch_rejectedImgPoints,
cameraMatrix = mtx,
distCoeffs = dist)
# Outline all of the markers detected in our image
# left camera view
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255))
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners_r, borderColor=(0, 0, 255))
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners_l, borderColor=(0, 0, 255))
QueryImg = aruco.drawDetectedMarkers(QueryImg, watch_corners, borderColor=(0, 0, 255))
# Valid flags
valid_board = False
valid_robot_r = False
valid_robot_l = False
valid_watch = False
# Detect the board, requires more than 3 markers on the board to be visible
if ids is not None and len(ids) > 3:
# Estimate the posture of the gridboard, seen from the left camera.
pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, mtx, dist)
if pose:
QueryImg = aruco.drawAxis(QueryImg, mtx, dist, rvec, tvec, board_marker_size)
board_to_cam_rot_mtx, board_origin = transform_from_board_to_camera(rvec, tvec, a_filter, board_origin)
valid_board = True
# detect robot base markers
if ids_r is not None and len(ids_r) >= 3:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners_r, robot_marker_size, mtx, dist)
for ident, rvec, tvec in zip(ids_r, rvecs, tvecs):
if ident[0] == 0 or ident[0] == 1 or ident[0] == 3:
t = transform_from_base_to_camera(rvec, tvec)
base_marker1, base_marker2, base_marker3 = apply_filter(a_filter, ident[0], t, base_marker1, base_marker2, base_marker3)
QueryImg = aruco.drawAxis(QueryImg, mtx, dist, rvec, tvec, 0.02)
x_axis, y_axis, z_axis, robot_origin = construct_new_axis_robot(base_marker2, base_marker1, base_marker3, False)
# rot_mat is from robot to camera
rot_mat = np.transpose(np.array([[x_axis[0], x_axis[1], x_axis[2]],[y_axis[0], y_axis[1], y_axis[2]], [z_axis[0], z_axis[1], z_axis[2]]]))
board_origin_in_robot_frame = compute_board_origin_in_base_frame(board_origin, rot_mat, robot_origin)
try:
Tb_r = compute_transformation_from_board_to_base(board_to_cam_rot_mtx, rot_mat, board_origin_in_robot_frame)
valid_robot_r = True
except:
pass
# detect left robot base markers
if ids_l is not None and len(ids_l) >= 3:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners_l, robot_left_marker_size, mtx, dist)
for ident, rvec, tvec in zip(ids_l, rvecs, tvecs):
#QueryImg = aruco.drawAxis(QueryImg, mtx, dist, rvec, tvec, 0.04)
if ident[0] == 0 or ident[0] == 2 or ident[0] == 3:
t = transform_from_base_to_camera(rvec, tvec)
base_marker_l1, base_marker_l2, base_marker_l3 = apply_filter(a_filter, ident[0], t, base_marker_l1, base_marker_l2, base_marker_l3)
QueryImg = aruco.drawAxis(QueryImg, mtx, dist, rvec, tvec, 0.02)
x_axis, y_axis, z_axis, robot_origin_l = construct_new_axis_robot(base_marker_l2, base_marker_l1, base_marker_l3, True)
# rot_mat_l is from robot to camera
rot_mat_l = np.transpose(np.array([[x_axis[0], x_axis[1], x_axis[2]],[y_axis[0], y_axis[1], y_axis[2]], [z_axis[0], z_axis[1], z_axis[2]]]))
board_origin_in_robot_frame = compute_board_origin_in_base_frame(board_origin, rot_mat_l, robot_origin_l)
try:
Tb_l = compute_transformation_from_board_to_base(board_to_cam_rot_mtx, rot_mat_l, board_origin_in_robot_frame)
valid_robot_l = True
except:
pass
# detect watch face markers
if watch_ids is not None and len(watch_ids) >= 4:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(watch_corners, watch_marker_size, mtx, dist)
for ident, rvec, tvec in zip(watch_ids, rvecs, tvecs):
if ident[0] == 0 or ident[0] == 1 or ident[0] == 3 or ident[0] == 2:
t = transform_from_base_to_camera(rvec, tvec)
watch_base_marker1, watch_base_marker2, watch_base_marker3, watch_base_marker4 = apply_filter_watch(a_filter, ident[0], t, watch_base_marker1, watch_base_marker2, watch_base_marker3, watch_base_marker4)
QueryImg = aruco.drawAxis(QueryImg, mtx, dist, rvec, tvec, 0.02)
BA = watch_base_marker2 - watch_base_marker1
DA = watch_base_marker3 - watch_base_marker1
normal = np.cross(DA, BA)
nnorm = np.linalg.norm(normal)
if nnorm > 1e-6:
normal /= nnorm
# compute the center point of all four markers
watch_origin = compute_center_from_four_points(watch_base_marker2, watch_base_marker3, watch_base_marker1, watch_base_marker4)
x_axis = watch_base_marker1 - watch_origin
x_axis /= np.linalg.norm(x_axis)
y_axis = np.cross(normal, x_axis)
rot_mat_w = np.transpose(np.array([[x_axis[0], x_axis[1], x_axis[2]],[y_axis[0], y_axis[1], y_axis[2]], [normal[0], normal[1], normal[2]]]))
board_origin_in_watch_frame = compute_board_origin_in_base_frame(board_origin, rot_mat_w, watch_origin)
try:
Tb_w = compute_transformation_from_board_to_base(board_to_cam_rot_mtx, rot_mat_w, board_origin_in_watch_frame)
valid_watch = True
except:
pass
self.pub_msg.header.stamp = rospy.Time.now()
self.pub2_msg.header.stamp = rospy.Time.now()
self.pub_watch_msg.header.stamp = rospy.Time.now()
if valid_robot_r:
Tb_r = np.linalg.inv(Tb_r)
T = np.zeros((4,4))
rad = 45.*np.pi/180.
c = np.cos(rad)
s = np.sin(rad)
T[0,0] = c
T[0,1] = -s
T[1,0] = s
T[1,1] = c
T[2,2] = 1
T[3,3] = 1
# print(T)
Tb_r = np.matmul(Tb_r, T)
else:
print(str(self.fr1) + ': Invalid right robot transformation. Using previous valid.')
self.pub_msg.pose.position.x = Tb_r[0,3]
self.pub_msg.pose.position.y = Tb_r[1,3]
self.pub_msg.pose.position.z = Tb_r[2,3]
T = copy.deepcopy(Tb_r)
T[0:3,3] = np.array([0,0,0])
quat_br = tf.transformations.quaternion_from_matrix(T)
quat_br = quat_br/np.linalg.norm(quat_br)
self.pub_msg.pose.orientation.x = quat_br[0]
self.pub_msg.pose.orientation.y = quat_br[1]
self.pub_msg.pose.orientation.z = quat_br[2]
self.pub_msg.pose.orientation.w = quat_br[3]
if valid_robot_l:
Tb_l = np.linalg.inv(Tb_l)
T = np.zeros((4,4))
rad = 45.*np.pi/180.
c = np.cos(rad)
s = np.sin(rad)
T[0,0] = c
T[0,1] = -s
T[1,0] = s
T[1,1] = c
T[2,2] = 1
T[3,3] = 1
# print(T)
Tb_l = np.matmul(Tb_l, T)
else:
print(str(self.fr1) + ': Invalid left robot transformation. Using previous valid.')
self.pub2_msg.pose.position.x = Tb_l[0,3]
self.pub2_msg.pose.position.y = Tb_l[1,3]
self.pub2_msg.pose.position.z = Tb_l[2,3]
T = copy.deepcopy(Tb_l)
T[0:3,3] = np.array([0,0,0])
quat_bl = tf.transformations.quaternion_from_matrix(T)
quat_bl = quat_bl/np.linalg.norm(quat_bl)
self.pub2_msg.pose.orientation.x = quat_bl[0]
self.pub2_msg.pose.orientation.y = quat_bl[1]
self.pub2_msg.pose.orientation.z = quat_bl[2]
self.pub2_msg.pose.orientation.w = quat_bl[3]
if valid_watch:
Tb_w = np.linalg.inv(Tb_w)
# T = np.zeros((4,4))
# rad = np.pi
# c = np.cos(rad)
# s = np.sin(rad)
# T[0,0] = c
# T[0,1] = -s
# T[1,0] = s
# T[1,1] = c
# T[2,2] = 1
# T[3,3] = 1
# # print(T)
# Tb_w = np.matmul(Tb_w, T)
else:
print(str(self.fr1) + ': Invalid watch face transformation. Using previous valid.')
self.pub_watch_msg.pose.position.x = Tb_w[0,3]
self.pub_watch_msg.pose.position.y = Tb_w[1,3]
self.pub_watch_msg.pose.position.z = Tb_w[2,3]
T = copy.deepcopy(Tb_w)
T[0:3,3] = np.array([0,0,0])
quat_bw = tf.transformations.quaternion_from_matrix(T)
quat_bw = quat_bw/np.linalg.norm(quat_bw)
self.pub_watch_msg.pose.orientation.x = quat_bw[0]
self.pub_watch_msg.pose.orientation.y = quat_bw[1]
self.pub_watch_msg.pose.orientation.z = quat_bw[2]
self.pub_watch_msg.pose.orientation.w = quat_bw[3]
self.pub.publish(self.pub_msg)
self.pub2.publish(self.pub2_msg)
self.pub_watch.publish(self.pub_watch_msg)
if not valid_board:
print(str(self.fr1) + ': Invalid board transformation. Using previous valid.')
self.br.sendTransform(Tb_r[0:3,3], quat_br, rospy.Time.now(), 'robot', 'board')
self.br.sendTransform(Tb_l[0:3,3], quat_bl, rospy.Time.now(), 'robot_left', 'board')
self.br.sendTransform(Tb_w[0:3,3], quat_bw, rospy.Time.now(), 'watch_face', 'board')
# # plot coordinate frame in 3D for debugging
# try:
# QueryImg = draw_axis(QueryImg, rot_mat, robot_origin, mtx, dist)
# except Exception as ex:
# print(ex)
# pass
# try:
# QueryImg = draw_axis(QueryImg, rot_mat_l, robot_origin_l, mtx, dist)
# except Exception as ex:
# print(ex)
# pass
# try:
# QueryImg = draw_axis(QueryImg, rot_mat_w, watch_origin, mtx, dist)
# except Exception as ex:
# print(ex)
# pass
# # cv2.imwrite('./out/img.png', QueryImg)
# cv2.imwrite('./out/imgA_{}.png'.format(self.fr1), QueryImg)
# cv2.imwrite('./out/imgB_{}.png'.format(self.fr2), QueryImg1)
rate.sleep()
def calibrateWithBoard(self, imgs, sensor, draw=False):
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_50)
# Create grid board object we're using in our stream
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=10,
squaresY=6,
squareLength=0.04, #in meters
markerLength=0.03, #in meters
dictionary=ARUCO_DICT)
# grayscale image
gray = cv2.cvtColor(imgs[sensor], cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=gray,
board=CHARUCO_BOARD,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=self.intrinsic[sensor],
distCoeffs=self.distCoeffs)
#print('Found {} corners in C{} sensor {}'.format(len(corners), self.camId, sensor))
imgs[sensor] = aruco.drawDetectedMarkers(imgs[sensor],
corners,
ids=ids,
borderColor=(0, 0, 255))
# Only try to find CharucoBoard if we found markers
if ids is not None and len(ids) > 10:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# Require more than 20 squares
if response is not None and response > 20:
# Estimate the posture of the charuco board, which is a construction of 3D space based on the 2D video
pose, self.extrinsic[sensor]['rvec'], self.extrinsic[sensor][
'tvec'] = aruco.estimatePoseCharucoBoard(
charucoCorners=charuco_corners,
charucoIds=charuco_ids,
board=CHARUCO_BOARD,
cameraMatrix=self.intrinsic[sensor],
distCoeffs=self.distCoeffs)
if draw:
imgs[sensor] = aruco.drawAxis(
imgs[sensor], self.intrinsic[sensor], self.distCoeffs,
self.extrinsic[sensor]['rvec'],
self.extrinsic[sensor]['tvec'], 2)
cv2.imwrite(
'./data/out/calib_C{}_{}.png'.format(
self.camId, sensor), imgs[sensor])
else:
print('Calibration board is not fully visible for C{} sensor: {}'.
format(self.camId, sensor))
assert 1 == 0
self.extrinsic[sensor]['rvec'] = cv2.Rodrigues(
self.extrinsic[sensor]['rvec'])[0]
self.extrinsic[sensor]['projMatrix'] = np.matmul(
self.intrinsic[sensor],
np.concatenate((self.extrinsic[sensor]['rvec'],
self.extrinsic[sensor]['tvec']),
axis=1))
# Захват кадра из видеопотока
ret, QueryImg = cam.read()
if ret == True:
# Перевод в GRAY
gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# Детектирование Aruco маркеров
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)
# Отрисовка контуров обнаруженных маркеров
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255))
# Если обнаружен маркер, то...
if ids is not None and len(ids) > 0:
# Определение его положения
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, ids, cameraMatrix, distCoeffs)
# Отрисовка куба на кадре
cube = draw(QueryImg, rvecs, tvecs, corners)
cv2.imshow('cube', cube)
def Video2():
# Load Calibrated Parameters
calibrationFile = "calibrationFileName.xml"
calibrationParams = cv2.FileStorage(calibrationFile, cv2.FILE_STORAGE_READ)
camera_matrix = calibrationParams.getNode("cameraMatrix").mat()
dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
r = calibrationParams.getNode("R").mat()
new_camera_matrix = calibrationParams.getNode("newCameraMatrix").mat()
image_size = (1920, 1080)
#map1, map2 = cv2.fisheye.initUndistortRectifyMap(camera_matrix, dist_coeffs, r, new_camera_matrix, image_size, cv2.CV_16SC2)
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_1000)
markerLength = 20 # Here, our measurement unit is centimetre.
markerSeparation = 4 # Here, our measurement unit is centimetre.
board = aruco.GridBoard_create(1, 1, markerLength, markerSeparation,
aruco_dict)
arucoParams = aruco.DetectorParameters_create()
img = board.draw(outSize=(800, 800))
#ShowImage(img)
videoFile = 0
cap = cv2.VideoCapture(videoFile)
while (True):
ret, frame = cap.read() # Capture frame-by-frame
if ret == True:
#frame_remapped = cv2.remap(frame, map1, map2, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT) # for fisheye remapping
frame_remapped_gray = cv2.cvtColor(
frame, cv2.COLOR_BGR2GRAY
) # aruco.detectMarkers() requires gray image
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame_remapped_gray, aruco_dict,
parameters=arucoParams) # First, detect markers
aruco.refineDetectedMarkers(frame_remapped_gray, board, corners,
ids, rejectedImgPoints)
if ids != None: # if there is at least one marker detected
im_with_aruco_board = aruco.drawDetectedMarkers(
frame, corners, ids, (0, 255, 0))
retval, rvec, tvec = aruco.estimatePoseBoard(
corners, ids, board, camera_matrix,
dist_coeffs) # posture estimation from a aruco board
if retval != 0:
x = np.array([[10], [-10], [0]], np.int32)
new_tvec = tvec + x
im_with_aruco_board = aruco.drawAxis(
im_with_aruco_board, camera_matrix, dist_coeffs, rvec,
new_tvec, 50
) # axis length 100 can be changed according to your requirement
else:
im_with_aruco_board = frame
cv2.imshow("arucoboard", im_with_aruco_board)
if cv2.waitKey(2) & 0xFF == ord('q'):
break
else:
break
cap.release() # When everything done, release the capture
cv2.destroyAllWindows()
while 1:
# Capture frame
ret, img = cap.read()
ret2, img2 = cap2.read()
# Convert to HSV
# img = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
# detect markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(img,
aruco_dict,
parameters=param)
corners2, ids2, rejectedImgPoints2 = aruco.detectMarkers(img2,
aruco_dict,
parameters=param)
aruco.refineDetectedMarkers(img, board, corners, ids, rejectedImgPoints,
cameraMatrix_left, distCoeff_left)
aruco.refineDetectedMarkers(img2, board, corners2, ids2,
rejectedImgPoints2, cameraMatrix_right,
distCoeff_right)
# estimate pose from Aruco board
if (ids is not None) and (ids2 is not None):
if img_count <= 1:
cv.imwrite(IMG_PATH + "left/left_aruco_" + id_label + '.png', img)
cv.imwrite(IMG_PATH + "right/right_aruco_" + id_label + '.png',
img2)
img_count += 1
img = aruco.drawDetectedMarkers(img, corners)
img2 = aruco.drawDetectedMarkers(img2, corners2)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
cameraMatrix_left,
def estimate_pose_board(self, img_gray, corners, ids, rejectedImgPoints, board):
nmarkers, rvec, tvec = estimatePoseBoard(corners, ids, board, camera_matrix, dist_coeffs, None, None, useExtrinsicGuess=False)
if nmarkers > 0:
corners2, ids2, _, _ = aruco.refineDetectedMarkers(img_gray, board, corners, ids, rejectedImgPoints, camera_matrix, dist_coeffs, parameters=arucoParams)
nmarkers, rvec, tvec = estimatePoseBoard(corners2, ids2, board, camera_matrix, dist_coeffs, None, None, useExtrinsicGuess=False)
return nmarkers, rvec, tvec
def get_webcam_reference(videoFilePath,
cParamsFilePath,
dictionary,
markerSize,
pose_timeout=5,
show_video=False):
"""
Function that returns the position and orientation of a marker from its
initial position. The input is a video file containing marker
"""
# Open video file and get number of frames
print('Preprocessing: counting number of frames')
n_frames = count_frames_manual(videoFilePath)
cap = cv2.VideoCapture(videoFilePath)
# Parameters from camera calibration
cal = pickle.load(open("tst_chessboard_lab.p", "rb"))
# cal = pickle.load(open(cParamsFilePath, "rb" ))
cMat = cal[0]
dist = cal[1]
# Initialze parameters to determine initial position
pose_0_set = False
pose_0 = ()
t_start = time.time()
t_set_pose_0 = t_start + pose_timeout
# Initialize collections
all_tvec = np.array([[], [], []])
all_rvec = np.array([[], [], []])
print('Postprocessing: tracking marker')
# Define the codec and create VideoWriter object
size = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
fourcc = cv2.VideoWriter_fourcc(*'DIVX')
out = cv2.VideoWriter('output.avi', fourcc, 29.0, size,
False) # 'False' for 1-ch instead of 3-ch for color
parameters = aruco.DetectorParameters_create(
) # Obtain detection parameters
# parameters.minCornerDistanceRate = 0.1
# parameters.minOtsuStdDev = 0.1
# parameters.maxErroneousBitsInBorderRate = 0.5
# parameters.adaptiveThreshConstant = 30
# parameters.perspectiveRemovePixelPerCell = 1000
# parameters.polygonalApproxAccuracyRate = 0.1
# Capture frame-by-frame
for i in range(n_frames):
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Convert to grayscale
gray = frame
# print(help(parameters))
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints, recoveredIdxs = aruco.refineDetectedMarkers(
gray,
board,
corners,
ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=cMat,
distCoeffs=dist)
if ids is not None:
# Obtain rotation and translation vectors
rvec, tvec = aruco.estimatePoseSingleMarkers(
corners, markerSize, cameraMatrix=cMat, distCoeffs=dist
) # corners, size of markers in meters, [3x3] intrinsic camera parameters, 5 distortion coefficients
rvec = np.array([rvec.item(0), rvec.item(1), rvec.item(2)])
tvec = np.array([tvec.item(0), tvec.item(1), tvec.item(2)])
if time.time() >= t_set_pose_0 and not pose_0_set:
pose_0 = (rvec, tvec)
pose_0_set = True
if pose_0_set:
rvec_0 = pose_0[0]
tvec_0 = pose_0[1]
tvec_n = tvec - tvec_0
rvec_n = rvec - rvec_0
t0 = np.append(all_tvec[0], tvec[0])
t1 = np.append(all_tvec[1], tvec[1])
t2 = np.append(all_tvec[2], tvec[2])
all_tvec = np.array([t0, t1, t2])
r0 = np.append(all_rvec[0], rvec[0])
r1 = np.append(all_rvec[1], rvec[1])
r2 = np.append(all_rvec[2], rvec[2])
all_rvec = np.array([r0, r1, r2])
r = np.sqrt(np.power(tvec_n[0], 2) + np.power(tvec_n[1], 2))
# print('Translation from initial position: ', tvec_n)
# print('Rotation from initial position: ', rvec_n)
# print('Total distance from initial position: ', r)
# show information on image
frameWithMarkers = aruco.drawDetectedMarkers(
gray, corners) # Draw marker borders
cv2.putText(frameWithMarkers, "ID: " + str(ids), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2,
cv2.LINE_AA) # Show detected IDs
aruco.drawAxis(frameWithMarkers, cMat, dist, rvec, tvec,
0.1) #Draw Axis
# Display the resulting frame
if show_video:
cv2.imshow('frame', frameWithMarkers)
out.write(gray)
else:
# Display: no IDs
cv2.putText(gray, "No IDs", (0, 64), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 255, 0), 2, cv2.LINE_AA)
if show_video:
cv2.imshow('frame', gray)
out.write(gray)
# Stop when q is pressed
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
out.release()
cv2.destroyAllWindows()
return all_tvec, all_rvec
while (cam.isOpened()):
# Capture current frame
ret, QueryImg = cam.read()
if ret == True:
# Convert image to grayscale
dst = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
dst, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=dst,
board=CHARUCO_BOARD,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
# Outline the markers
QueryImg = aruco.drawDetectedMarkers(QueryImg,
corners,
borderColor=(0, 0, 255))
# Only try to find CharucoBoard if markers were detected
if ids is not None and len(ids) > 10:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
def findCharucoBoard(self, req):
self.rvecs_arr = np.zeros((3, NUMBER))
self.tvecs_arr = np.zeros((3, NUMBER))
res = aruco_infoResponse()
for order in range(NUMBER):
# Capturing each frame of our video stream
# ret, self.QueryImg = self.cam.read()
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
self.QueryImg = np.asanyarray(color_frame.get_data())
# grayscale image
gray = cv2.cvtColor(self.QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_param)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=gray,
board=self.charuco_board,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=self.cameraMatrix,
distCoeffs=self.distCoeffs)
# Require 15 markers before drawing axis
if ids is not None and len(ids) > 10:
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=self.charuco_board)
# Require more than 20 squares
if response is not None and response > 20:
# Estimate the posture of the charuco board, which is a construction of 3D space based on the 2D video
pose, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners=charuco_corners,
charucoIds=charuco_ids,
board=self.charuco_board,
cameraMatrix=self.cameraMatrix,
distCoeffs=self.distCoeffs)
if pose:
if order == 0:
print(
"============================================="
)
print(rvec)
print(tvec)
for i in range(3):
self.rvecs_arr[i][order] = rvec[i][0]
self.tvecs_arr[i][order] = tvec[i][0]
# self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.02)
cv2.waitKey(10)
# Display our image
# print('self.rvecs_arr = ', self.rvecs_arr)
# print('self.tvecs_arr = ', self.tvecs_arr)
cv2.destroyAllWindows()
r_avg = np.zeros(3)
t_avg = np.zeros(3)
ra = self.rvecs_arr[0].nonzero()
rb = self.rvecs_arr[1].nonzero()
rc = self.rvecs_arr[2].nonzero()
tx = self.tvecs_arr[0].nonzero()
ty = self.tvecs_arr[1].nonzero()
tz = self.tvecs_arr[2].nonzero()
ra = self.rvecs_arr[0][ra]
rb = self.rvecs_arr[1][rb]
rc = self.rvecs_arr[2][rc]
tx = self.tvecs_arr[0][tx]
ty = self.tvecs_arr[1][ty]
tz = self.tvecs_arr[2][tz]
ra = np.sort(ra, kind='quicksort')
rb = np.sort(rb, kind='quicksort')
rc = np.sort(rc, kind='quicksort')
tx = np.sort(tx, kind='quicksort')
ty = np.sort(ty, kind='quicksort')
tz = np.sort(tz, kind='quicksort')
r = np.array((ra, rb, rc))
t = np.array((tx, ty, tz))
for i in range(3):
rv, tv = r[i], t[i]
while np.std(rv) > 0.01 and len(rv) >= NUMBER * 0.2:
if abs(rv[0] - np.average(rv)) > abs(rv[-1] - np.average(rv)):
rv = np.delete(rv, 0)
else:
rv = np.delete(rv, -1)
while np.std(tv) > 0.01 and len(tv) >= NUMBER * 0.2:
if abs(tv[0] - np.average(tv)) > abs(tv[-1] - np.average(tv)):
tv = np.delete(tv, 0)
else:
tv = np.delete(tv, -1)
r_avg[i] = np.average(rv)
t_avg[i] = np.average(tv)
res.rvecs = r_avg
res.tvecs = t_avg
print('res.rvecs is ', res.rvecs)
print('res.tvecs is ', res.tvecs)
result = np.array(())
result = np.append(result, [np.copy(r_avg), np.copy(t_avg)])
result = result.reshape(2, 3)
filename = self.curr_path + "/pic/charucoboard-pic-" + str(
int(self.frameId)) + ".jpg"
cv2.imwrite(filename, self.QueryImg)
# Outline all of the markers detected in our image
self.QueryImg = aruco.drawDetectedMarkers(self.QueryImg,
corners,
borderColor=(0, 0, 255))
self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix,
self.distCoeffs, result[0], result[1],
0.02)
# self.curr_path = os.path.dirname(os.path.abspath(__file__))
filename = self.curr_path + "/pic/detection result-" + str(
int(self.frameId)) + ".jpg"
cv2.imwrite(filename, self.QueryImg)
self.frameId += 1
print('------')
return res
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()
def findCharucoBoard(self, req):
self.rvecs_arr = np.zeros((3, NUMBER))
self.tvecs_arr = np.zeros((3, NUMBER))
res = aruco_infoResponse()
for order in range(NUMBER):
# Capturing each frame of our video stream
# ret, self.QueryImg = self.cam.read()
# frames = self.pipeline.wait_for_frames()
# color_frame = frames.get_color_frame()
# self.QueryImg = np.asanyarray(color_frame.get_data())
# grayscale image
gray = cv2.cvtColor(self.QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=gray,
board=CHARUCO_BOARD,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=self.cameraMatrix,
distCoeffs=self.distCoeffs)
# Require 15 markers before drawing axis
if ids is not None and len(ids) > 10:
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# Require more than 20 squares
if response is not None and response > 20:
# Estimate the posture of the charuco board, which is a construction of 3D space based on the 2D video
pose, rvec_, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners=charuco_corners,
charucoIds=charuco_ids,
board=CHARUCO_BOARD,
cameraMatrix=self.cameraMatrix,
distCoeffs=self.distCoeffs,
rvec=np.zeros(3),
tvec=np.zeros(3))
# self.rvecs, self.tvecs = aruco.estimatePoseSingleMarkers(corners, self.markersize, self.cameraMatrix, self.distCoeffs)
# for _id, rvec, tvec in zip(ids, self.rvecs, self.tvecs):
if pose:
print(rvec_)
print(tvec)
if order == 0:
print(
"============================================="
)
print(rvec_)
print(tvec)
for i in range(3):
self.rvecs_arr[i][order] = rvec_[i]
self.tvecs_arr[i][order] = tvec[i]
# self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.02)
cv2.waitKey(10)
# Display our image
# print('self.rvecs_arr = ', self.rvecs_arr)
# print('self.tvecs_arr = ', self.tvecs_arr)
cv2.destroyAllWindows()
r_avg = np.zeros(3)
t_avg = np.zeros(3)
ra = self.rvecs_arr[0].nonzero()
rb = self.rvecs_arr[1].nonzero()
rc = self.rvecs_arr[2].nonzero()
tx = self.tvecs_arr[0].nonzero()
ty = self.tvecs_arr[1].nonzero()
tz = self.tvecs_arr[2].nonzero()
ra = self.rvecs_arr[0][ra]
rb = self.rvecs_arr[1][rb]
rc = self.rvecs_arr[2][rc]
tx = self.tvecs_arr[0][tx]
ty = self.tvecs_arr[1][ty]
tz = self.tvecs_arr[2][tz]
ra = np.sort(ra, kind='quicksort')
rb = np.sort(rb, kind='quicksort')
rc = np.sort(rc, kind='quicksort')
tx = np.sort(tx, kind='quicksort')
ty = np.sort(ty, kind='quicksort')
tz = np.sort(tz, kind='quicksort')
r = np.array((ra, rb, rc))
t = np.array((tx, ty, tz))
for i in range(3):
rv, tv = r[i], t[i]
while np.std(rv) > 0.01 and len(rv) >= NUMBER * 0.2:
if abs(rv[0] - np.average(rv)) > abs(rv[-1] - np.average(rv)):
rv = np.delete(rv, 0)
else:
rv = np.delete(rv, -1)
while np.std(tv) > 0.01 and len(tv) >= NUMBER * 0.2:
if abs(tv[0] - np.average(tv)) > abs(tv[-1] - np.average(tv)):
tv = np.delete(tv, 0)
else:
tv = np.delete(tv, -1)
r_avg[i] = np.average(rv)
t_avg[i] = np.average(tv)
# print('[_id, r,t] = ', [_id, r,t])
# res.ids.append(_id)
# res.rvecs = np.append(res.rvecs, r_avg)
# res.tvecs = np.append(res.tvecs, t_avg)
res.rvecs = r_avg
res.tvecs = t_avg
print('res.rvecs is ', res.rvecs)
print('res.tvecs is ', res.tvecs)
result = np.array(())
result = np.append(result, [np.copy(r_avg), np.copy(t_avg)])
result = result.reshape(2, 3)
if self.name == 'test':
# Outline all of the markers detected in our image
self.QueryImg = aruco.drawDetectedMarkers(self.QueryImg,
corners,
borderColor=(0, 0, 255))
self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix,
self.distCoeffs, result[0],
result[1], 0.02)
curr_path = os.path.dirname(os.path.abspath(__file__))
filename = curr_path + "/pic/camera-pic-of-charucoboard-" + str(
int(self.frameId)) + ".jpg"
cv2.imwrite(filename, self.QueryImg)
self.frameId += 1
# cv2.imwrite('./123%d.jpg'%self.cnd, self.QueryImg)
# self.cnd += 1
# cv2.namedWindow('Amanda', cv2.WINDOW_AUTOSIZE)
# self.QueryImg = cv2.imread('./123%d.jpg'%self.cnd)
# cv2.imshow('Amanda', self.QueryImg)
# cv2.waitKey(1000)
# cv2.destroyAllWindows()
# time.sleep(2)
print('------')
# while not cv2.waitKey(1) & 0xFF == ord('q'):
# pass
# cv2.destroyAllWindows()
print("self.cameraMatrix.reshape(1,9):",
self.cameraMatrix.reshape(1, 9)[0])
res.camera_mat = self.cameraMatrix.reshape(1, 9)[0]
return res
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 get_webcam_reference(video_file, cam_params_file, dictionary, marker_size, board, show_video=False, save_output=False, output_file_name='output.avi', webcam_stream=False):
"""
Function that returns the position and orientation of a marker from its
initial position.
INPUTS:
- video_file: path to a video file to be processed
- cam_params_file: pickle file containing parameters from camera calibration
- dictionary: aruco predifined dictionary used to generate markers
- board: aruco marker board
- marker_size: size of marker to detect in meters
- show_video (default=False): play video with detection results. Video playback can be stopped by pressing q.
- save_output (default=False): save the detection output to a video file
- output_file_name (default='output.avi'): name of output video file
OUTPUTS:
- all_tvec: marker coordinates of each frame [x, y, z]
- all_rvec: marker orientations of each frame [x, y, z]
"""
# Open video file and get number of frames
if webcam_stream:
cap = cv2.VideoCapture(0)
else:
print('Preprocessing: counting number of frames')
n_frames = count_frames_manual(video_file)
cap = cv2.VideoCapture(video_file)
loop = True # Stop condition for while loop
i = 0 # Current iteration. Loop needs to stop when all frames of videofile have been processed
# Parameters from camera calibration
cal = pickle.load(open(cam_params_file, "rb" ))
cMat = cal[0]
dist = cal[1][0]
# Initialize collections
all_tvec = list()
all_rvec = list()
print('Tracking marker')
if save_output:
# Define the codec and create VideoWriter object
size = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)), int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
fourcc = cv2.VideoWriter_fourcc(*'DIVX')
out = cv2.VideoWriter(output_file_name,fourcc, 29.0, size, True) # 'False' for 1-ch instead of 3-ch for color
parameters = aruco.DetectorParameters_create() # Obtain detection parameters
while(loop):
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Convert to grayscale
gray = frame
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints, recoveredIdxs = aruco.refineDetectedMarkers(gray, board, corners, ids, rejectedCorners=rejectedImgPoints, cameraMatrix=cMat, distCoeffs=dist)
if ids is not None:
# Obtain rotation and translation vectors
rObject, tObject, _objPoints = aruco.estimatePoseSingleMarkers(corners, marker_size, cameraMatrix=cMat, distCoeffs=dist) # corners, size of markers in meters, [3x3] intrinsic camera parameters, 5 distortion coefficients
rvec = [rObject.item(0), rObject.item(1), rObject.item(2)]
tvec = [tObject.item(0), tObject.item(1), tObject.item(2)]
all_rvec.append(rvec)
all_tvec.append(tvec)
# show information on image
frameWithMarkers = aruco.drawDetectedMarkers(gray, corners, ids=ids) # Draw marker borders
cv2.putText(frameWithMarkers, "ID: " + str(ids), (0,64), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 2, cv2.LINE_AA) # Show detected IDs
aruco.drawAxis(frameWithMarkers, cMat, dist, np.array(rvec), np.array(tvec), 0.1) #Draw Axis
# Display the resulting frame
if show_video:
cv2.imshow('frame',frameWithMarkers)
else:
# Display: no IDs
cv2.putText(gray, "No IDs", (0,64), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0),2,cv2.LINE_AA)
if show_video:
cv2.imshow('frame',gray)
if save_output:
out.write(gray)
# Stop when q is pressed
if cv2.waitKey(1) & 0xFF == ord('q'):
loop=False
i += 1
if not webcam_stream:
if i == n_frames:
loop = False
# When everything done, release the capture
cap.release()
if save_output:
out.release()
cv2.destroyAllWindows()
return np.array(all_tvec), np.array(all_rvec)
ret, frame = cap.read() # Capture frame-by-frame
print(frame.shape)
# operations on the frame
if ret == True:
gray = cv2.cvtColor(
frame,
cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image
CHARUCO_PARAMS.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, CHARUCO_DICT, parameters=CHARUCO_PARAMS)
aruco.refineDetectedMarkers(gray, CHARUCO_BOARD, corners, ids,
rejectedImgPoints)
if np.all(ids != None): # if there is at least one marker detected
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, gray, CHARUCO_BOARD)
# try to create a birds eye view of the scene based on the charuco recognition
try:
main_corners = get_boundaries(charucoCorners, frame.shape[0])
# pbar.set_description(f"corners: {main_corners}")
for item in main_corners:
draw_point(frame, (int(item[0, 0]), int(item[0, 1])))
print(f"corners: {main_corners}")
new_corners = get_box(main_corners)
def get_camera_pose_from_single_markerboard(self, msg):
n_frame = msg.n_frame
rospy.loginfo("Get camera pose of {} from markerboard".format(self.camera_name))
header_frame_id = "{}_rgb_camera_link".format(self.camera_name)
# basic parameters
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) # dictionary id
board = aruco.GridBoard_create(5, 7, 0.033, 0.004, dictionary)
parameters = aruco.DetectorParameters_create()
camera_info = rospy.wait_for_message("/{}/rgb/camera_info".format(self.camera_name), CameraInfo)
K = np.array(camera_info.K).reshape(3, 3)
D = np.array(camera_info.D)
pos_list = []
quat_list = []
n_sucess = 0
# detect marker from image
for n in range(n_frame):
image = rospy.wait_for_message("/{}/rgb/image_raw".format(self.camera_name), Image)
bridge = cv_bridge.CvBridge()
frame = bridge.imgmsg_to_cv2(image, desired_encoding='bgr8')
corners, ids, rejected = aruco.detectMarkers(frame, dictionary, parameters=parameters)
corners, ids, rejected, recovered = aruco.refineDetectedMarkers(frame, board, corners, ids, rejected,
K, D,
errorCorrectionRate=-1,
parameters=parameters)
N, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, K, D, None, None)
if N:
cv2.aruco.drawAxis(frame, K, D, rvec, tvec, 0.2)
cv2.aruco.drawDetectedMarkers(frame, corners, ids)
self.aruco_img_pub.publish(bridge.cv2_to_imgmsg(frame))
pos = tvec
rot = np.eye(4)
rot[:3, :3] = np.squeeze(cv2.Rodrigues(rvec)[0])
quat = t.quaternion_from_matrix(rot)
pos_list.append(pos)
quat_list.append(quat)
n_sucess += 1
if len(pos_list) == 0:
rospy.logwarn("Failed to detect the marker board")
return False
pos, quat = orh.average_pq(pos_list, quat_list)
source_frame = header_frame_id
target_frame = "{}_markerboard".format(self.camera_name)
static_tf_min = orh.pq_to_transform_stamped(pos, quat, source_frame, target_frame)
# source_frame = "{}_markerboard".format(self.camera_name)
# target_frame = header_frame_id
# static_tf_min = orh.pq_to_transform_stamped(pos, quat, source_frame, target_frame)
self.static_aruco_tfs.append(static_tf_min)
rospy.loginfo("Publish static tf: {} -> {}_markerboard from ArUco".format(header_frame_id, self.camera_name))
# find target marker in world map
target_marker = None
for marker in self.world_map["markers"]:
if marker["id"] == "board":
target_marker = marker
if target_marker is None:
rospy.logwarn("No information in world map for marker board")
pos = target_marker["position"]
pos = [-p for p in pos]
quat = target_marker["orientation"] # TODO: invert quaternion
source_frame = "{}_markerboard".format(self.camera_name)
target_frame = "world"
static_tf_world_to_fid = orh.pq_to_transform_stamped(pos, quat, source_frame, target_frame)
self.static_world_tfs.append(static_tf_world_to_fid)
if msg.publish_worldmap:
rospy.loginfo("Publish static tf: {}_markerboard -> world from world map ".format(self.camera_name))
self.br.sendTransform(self.static_aruco_tfs + self.static_world_tfs)
else:
self.br.sendTransform(self.static_aruco_tfs)
self.save_transfrom_as_json("world", "{}_rgb_camera_link".format(self.camera_name))
rospy.loginfo("Finished the camera pose calibration")
return True
def image_callback(self, data):
# --- SETUP STUFF --- #
# Update the board we're looking for
self.update_aruco_target()
# Define variables for messages to publish
pose_msg = Pose()
bool_msg = Bool()
avg_corners = 0
# --- IMAGE PROCESSING --- #
# Convert from ROS message to OpenCV image
cv_image = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
# Find corners and IDs of aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(cv_image, ARUCO_DICT, parameters=ARUCO_PARAM)
# Refine for improved accuracy
aruco.refineDetectedMarkers(cv_image, SOUTH_2x1_BOARD, corners, ids, rejectedImgPoints, self.cmatx, self.dist)
# If markers found, estimate pose
if ids is not None:
aruco.drawDetectedMarkers(cv_image, corners, ids)
if self.prev_rvec is None:
_, self.prev_rvec, self.prev_tvec = aruco.estimatePoseBoard(corners, ids, SOUTH_2x1_BOARD, self.cmatx, self.dist)
# Compute pose estimate based on board corner positions
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, SOUTH_2x1_BOARD, self.cmatx, self.dist, rvec=self.prev_rvec, tvec=self.prev_tvec, useExtrinsicGuess=True)
# If succesful, convert rvec from rpy to quaternion, fill pose message
if retval != 0:
# Set param to true
self.first_marker_detected = True
self.prev_tvec = tvec
self.prev_rvec = rvec
# Compute pose of Camera relative to world
dst, _ = cv2.Rodrigues(rvec)
R = dst.T
tvec = np.dot(-R, tvec)
rvec, _ = cv2.Rodrigues(R)
rospy.loginfo(rvec.shape)
# Convert the 'rvec' from rpy to a quaternion
quat = tf.transformations.quaternion_from_euler(rvec[1, 0], rvec[0, 0], -rvec[2, 0])
# Store pose and quaternion information of Aruco Board
pose_msg.position.x = tvec[2, 0]
pose_msg.position.y = tvec[0, 0]
pose_msg.position.z = tvec[1, 0]
pose_msg.orientation.x = quat[0]
pose_msg.orientation.y = quat[1]
pose_msg.orientation.z = quat[2]
pose_msg.orientation.w = quat[3]
# Calculate the average x distance between the corners
min = corners[0][0, 0, 0]
max = corners[0][0, 0, 0]
for corner in corners:
if np.min(corner[0, :, 0]) < min:
min = np.min(corner[0, :, 0])
elif np.max(corner[0, :, 0]) > max:
max = np.max(corner[0, :, 0])
# Publish pose estimate
self.pose_pub.publish(pose_msg)
self.avg_of_corners.publish((min + max) / 2.0)
# Update boolean message
bool_msg.data = retval
else:
bool_msg.data = False
cv2.imshow("image", cv_image)
cv2.waitKey(1)
# Always publish whether markers were found or not
self.avg_of_corners.publish(avg_corners)
self.bool_pub.publish(bool_msg)
(ret, frame) = cap.read()
# frame = frame[300:640,100:560]
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# cv2.imshow('original', frame)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
#cv2.imshow('grayed', gray)
# ret3,gray = cv2.threshold(gray,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
# cv2.imshow("thresholded", gray)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=arucoParams) # Detect aruco
aruco.refineDetectedMarkers(gray, board, corners, ids, rejectedImgPoints)
if ids is not None: # if the aruco marker detected
# rvec, tvec, objpoints = aruco.estimatePoseSingleMarkers(corners, markerLength, camera_Matrix, dist_Coff) # For a single marker
# board = aruco.Board_create(objpoints,dictionary,ids)
imgWithAruco = aruco.drawDetectedMarkers(frame, corners, ids,
(0, 255, 0))
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_Matrix, dist_Coff)
# imgWithAruco = aruco.drawAxis(frame, camera_Matrix, dist_Coff, rvec, tvec, 0.1)
# imgWithArucwo = aruco.drawAxis(imgWithAruco, camera_Matrix, dist_Coff, rvec, tvec, 10) #balash weghet nazar abo balash katar meno # axis length 100 can be changed according to your requirement
x_dist = round(tvec[0][0] * 100, 1)
y_dist = round(tvec[1][0] * 100, 1)
z_dist = round(tvec[2][0] * 100, 1)
x_rot = round(rvec[0][0], 2)
def calibrate_camera_aruco(self,
samples_dir="frames",
outputfile="camera_matrix_aruco.yaml"):
self.outputfile = outputfile
board = aruco.GridBoard_create(self.MarkerX, self.MarkerY, 0.05, 0.01,
self.aruco_dict)
cam_matrix, dist_coeff, rvecs, tvecs = self.read_cam_matrix(outputfile)
print("dist_coeff original")
print(dist_coeff)
print("cam_matrix original")
print(cam_matrix)
im = Imutils_Master()
all_img = []
all_obj = []
h, w = 0, 0
file_count = 0
total_makers_to_look = self.MarkerX * self.MarkerY
for fname in os.listdir(samples_dir):
file_count += 1
img = cv2.imread(samples_dir + "/" + fname)
h, w = img.shape[:2]
gray = im.gray(img)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.parameters)
corners1, ids1, rejectedImgPoints1, rec_index = aruco.refineDetectedMarkers(
gray, board, corners, ids, rejectedImgPoints)
getcount = self.get_marker_count(ids1)
if ids1 is not None and getcount == total_makers_to_look:
ret, rv, tv = aruco.estimatePoseBoard(corners1, ids1, board,
cam_matrix, dist_coeff)
if ret > 0:
aruco.drawDetectedMarkers(img, corners, ids, (0, 0, 255))
obj_points, img_points = aruco.getBoardObjectAndImagePoints(
board, corners1, ids1)
all_obj.append(obj_points)
all_img.append(img_points)
else:
print("not able to estimate board")
cv2.imshow("frame", img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
print("calibrating starts... may take while")
if len(all_obj) == len(all_img):
rms, cam_matrix, dist_coeff, rvecs, tvecs = cv2.calibrateCamera(
all_obj, all_img, (w, h), None, None)
if rms:
data = {
'camera_matrix': np.asarray(cam_matrix).tolist(),
'dist_coeff': np.asarray(dist_coeff).tolist(),
'rvecs': np.asarray(rvecs).tolist(),
'tvecs': np.asarray(tvecs).tolist()
}
flag = self.write_cam_matrix(outputfile, data)
if flag:
print("camera details is written to file")
cam_matrix, dist_coeff, rvecs, tvecs = self.read_cam_matrix(
outputfile)
print("new cam_matrix")
print(cam_matrix)
print("new dist_coeff")
print(dist_coeff)
else:
print("error writing camera details to file")
else:
print(
"Number of object points is not equal to the number of samples taken"
)
print(len(all_obj))
print(len(all_img))
cap = cv2.VideoCapture(videoFile)
while (True):
ret, frame = cap.read() # Capture frame-by-frame
if ret == True:
frame_remapped = cv2.remap(
frame, map1, map2, cv2.INTER_LINEAR,
cv2.BORDER_CONSTANT) # for fisheye remapping
frame_remapped_gray = cv2.cvtColor(
frame_remapped,
cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame_remapped_gray, aruco_dict,
parameters=arucoParams) # First, detect markers
aruco.refineDetectedMarkers(frame_remapped_gray, board, corners, ids,
rejectedImgPoints)
if ids != None: # if there is at least one marker detected
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, frame_remapped_gray, board)
im_with_charuco_board = aruco.drawDetectedCornersCharuco(
frame_remapped, charucoCorners, charucoIds, (0, 255, 0))
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, board, camera_matrix,
dist_coeffs) # posture estimation from a charuco board
if retval == True:
im_with_charuco_board = aruco.drawAxis(
im_with_charuco_board, camera_matrix, dist_coeffs, rvec,
tvec, 100
) # axis length 100 can be changed according to your requirement
else:
def show_webcam(mirror=False):
global last_x, last_y, last_z, last_x_unfiltered, last_pitch_unfiltered, last_time, last_timestamps
vs = VideoStream(src=0).start()
slow_z = 0
while True:
img, cappos, capfps = vs.read()
utcnowms = cappos #int((datetime.datetime.utcnow() - datetime.datetime(1970, 1, 1)).total_seconds() * 1000)
fps = 0
if last_time != None:
if last_time == utcnowms:
continue
fps = 1000 / (utcnowms - last_time)
last_time = utcnowms
#(h, w) = img.shape[:2]
#print(h,w)
#center = (w/2,h/2)
#M = cv2.getRotationMatrix2D(center, 180, 1)
#img = cv2.warpAffine(img, M, (w, h))
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=arucoParams)
if ids is not None: # if aruco marker detected
#rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, markerLength, camera_matrix, dist_coeffs) # For a single marker
img = aruco.drawDetectedMarkers(img, corners, ids, (0, 255, 0))
v, rvec, tvec = aruco.estimatePoseBoard(corners,
ids,
board_small,
camera_matrix,
dist_coeffs,
None,
None,
useExtrinsicGuess=False)
#print("board small", v, "rvec", rvec, "tvec", tvec)
if v > 0:
corners2, ids2, rejected2, recovered2 = aruco.refineDetectedMarkers(
gray,
board_small,
corners,
ids,
rejectedImgPoints,
camera_matrix,
dist_coeffs,
parameters=arucoParams)
v, rvec, tvec = aruco.estimatePoseBoard(
corners2,
ids2,
board_small,
camera_matrix,
dist_coeffs,
None,
None,
useExtrinsicGuess=False)
#img = aruco.drawAxis(img, camera_matrix, dist_coeffs, rvec, tvec, 3.21 / 2)
# move small_board origin to slightly forward to robot body origin
small_board_center = (smbm_len + smbm_sep / 2) # cm, see above
tag_to_body_center_height = 0 #2 + 0.5 # cm
rot, _ = cv2.Rodrigues(rvec)
centering_vec = np.dot(
rot,
np.array([[
small_board_center + 2, small_board_center,
-tag_to_body_center_height
]]).T)
tvec[0] += centering_vec[0]
tvec[1] += centering_vec[1]
tvec[2] += centering_vec[2]
img = aruco.drawAxis(img, camera_matrix, dist_coeffs, rvec,
tvec, 3.21 / 2)
v2, rvec2, tvec2 = aruco.estimatePoseBoard(corners,
ids,
board_big,
camera_matrix,
dist_coeffs,
None,
None,
useExtrinsicGuess=False)
#print("board big", v2, rvec2, tvec2)
if v2 > 0:
corners2, ids2, rejected2, recovered2 = aruco.refineDetectedMarkers(
gray,
board_big,
corners,
ids,
rejectedImgPoints,
camera_matrix,
dist_coeffs,
parameters=arucoParams)
v2, rvec2, tvec2 = aruco.estimatePoseBoard(
corners2,
ids2,
board_big,
camera_matrix,
dist_coeffs,
None,
None,
useExtrinsicGuess=False)
img = aruco.drawAxis(img, camera_matrix, dist_coeffs, rvec2,
tvec2, 3.21 / 2)
t = None # 16
m = None # 684, 305
if False:
for i, e in enumerate(ids):
#print(e)
if e == 684:
img = aruco.drawAxis(img, camera_matrix, dist_coeffs,
rvec[i], tvec[i], 4.8 / 2)
t = [rvec[i], tvec[i]]
if e == 305:
m = [rvec[i], tvec[i]]
if v > 0 and v2 > 0:
# both the floor board (v2) and agent board (v) have been detected
rv, tv = get_relative_position(rvec, tvec, rvec2, tvec2)
#print("tv", tv)
#print("rv", rv)
#print("dist", np.linalg.norm(tv))
dist = np.linalg.norm(tv)
if True:
linedist = 40
cv2.putText(img,
"Ant distance from reference to ant: %2.1fcm" %
dist, (20, 40),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=linedist / 30,
color=(0, 0, 255),
thickness=2)
cv2.putText(
img,
"Ant position: x: %2.1fcm y: %2.1fcm z: %2.1fcm" %
(tv[0], tv[1], tv[2]), (20, 40 + linedist),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=linedist / 30,
color=(0, 0, 255),
thickness=2)
cv2.putText(
img,
"Ant orientation: roll: %2.1fdeg pitch: %2.1fdeg yaw: %2.1fdeg"
% (rv[0] / np.pi * 180, rv[1] / np.pi * 180,
rv[2] / np.pi * 180), (20, 40 + linedist * 2),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=linedist / 30,
color=(0, 0, 255),
thickness=2)
a = 0.3
slow_z = a * tv[2] + (1 - a) * slow_z
d = OrderedDict()
d["id"] = "external_tag_tracking_camera"
d["server_epoch_ms"] = utcnowms
d["dist"] = float(dist)
d["x"] = float(tv[0]) / 100. # in m
d["y"] = float(tv[1]) / 100. # in m
d["z"] = float(slow_z) / 100. # in m
d["roll"] = float(rv[0] / np.pi * 180)
d["pitch"] = float(rv[1] / np.pi * 180)
d["yaw"] = float(rv[2] / np.pi * 180)
d["sent"] = False
# filtering
last_x_unfiltered.append(d["x"])
last_x_unfiltered = last_x_unfiltered[-3:]
if len(last_x_unfiltered) > 2:
xabsmaxdiff = np.abs(np.amax(np.diff(last_x_unfiltered)))
else:
xabsmaxdiff = 0
last_pitch_unfiltered.append(d["pitch"])
last_pitch_unfiltered = last_pitch_unfiltered[-3:]
if len(last_pitch_unfiltered) > 2:
pitchabsmaxdiff = np.abs(
np.amax(np.diff(last_pitch_unfiltered)))
else:
pitchabsmaxdiff = 0
if d["z"] < -0.05:
print("negative z, less than -0.05m, ignoring")
elif xabsmaxdiff > 0.15:
print("more than 15cm jump in x, ignoring")
elif pitchabsmaxdiff > 20:
print("more than 20deg jump in pitch, ignoring")
else:
N = 5
last_x.append(d["x"])
last_x = last_x[-N:]
last_y.append(d["y"])
last_y = last_y[-N:]
last_z.append(d["z"])
last_z = last_z[-N:]
last_timestamps.append(d["server_epoch_ms"] * 1e-3) # in s
last_timestamps = last_timestamps[-N:]
if len(last_x) >= N:
xvel = (last_x[-1] - last_x[-2]) / (
last_timestamps[-1] - last_timestamps[-2])
xvel_hd5 = holoborodko_diff(
np.array(last_x),
float(np.mean(np.diff(np.array(last_timestamps)))))
d["xvel_raw"] = xvel
d["xvel_hd5"] = xvel_hd5
d["xvel"] = d["xvel_hd5"]
yvel = (last_y[-1] - last_y[-2]) / (
last_timestamps[-1] - last_timestamps[-2])
yvel_hd5 = holoborodko_diff(
np.array(last_y),
float(np.mean(np.diff(np.array(last_timestamps)))))
d["yvel_raw"] = yvel
d["yvel_hd5"] = yvel_hd5
d["yvel"] = d["yvel_hd5"]
zvel = (last_z[-1] - last_z[-2]) / (
last_timestamps[-1] - last_timestamps[-2])
zvel_hd5 = holoborodko_diff(
np.array(last_z),
float(np.mean(np.diff(np.array(last_timestamps)))))
d["zvel_raw"] = zvel
d["zvel_hd5"] = zvel_hd5
d["zvel"] = d["zvel_hd5"]
sock.send_json(d)
d["sent"] = True
print("fps %5.1f " % fps, end='')
print(
"- %d dist %2.1f x %1.3fm y %1.3fm z %1.3fm roll %3.0f pitch %3.0f yaw %3.0f"
% tuple([
d[x] for x in [
"server_epoch_ms", "dist", "x", "y", "z", "roll",
"pitch", "yaw"
]
]),
end='')
if "xvel" in d:
print(" xvel%6.3f %6.3f y%6.3f z%6.3f" %
(d["xvel"], d["xvel_raw"], d["yvel"], d["zvel"]))
else:
print()
logfile.write(json.dumps(d) + '\n')
if mirror:
img = cv2.flip(img, 1)
if True:
cv2.imshow('ant', img)
if cv2.waitKey(1) == 27:
break # esc to quit
cv2.destroyAllWindows()
