def StreamProcessedImages(self, request, context):
try:
detector = Detector(searchpath=['/usr/local/lib'],
families='tag36h11',
nthreads=3,
quad_decimate=1.0,
quad_sigma=0.8,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
with picamera.PiCamera(resolution=(width, height),
framerate=1) as camera:
# camera.resolution = (width, height)
print("camera", camera)
# Start a preview and let the camera warm up for 2 seconds
camera.start_preview()
time.sleep(2)
with picamera.array.PiRGBArray(camera) as stream:
for foo in camera.capture_continuous(stream,
'rgb',
use_video_port=True):
stream.flush()
# gray = np.mean(stream.array, axis=2, dtype=np.uint8)
gray = rgb2gray(stream.array)
K = (329.8729619143081, 332.94611303946357, 528.0,
396.0)
detections = detector.detect(gray,
estimate_tag_pose=True,
camera_params=K,
tag_size=0.08)
print("gray.shape", gray.shape)
print("stream.array.shape", stream.array.shape)
gray3 = np.zeros((height, width, 3))
gray3[:, :, 1] = gray
print("detected {} images".format(len(detections)))
image_data = stream.array
# image_data = image_data.reshape((320, 2,
# 240, 2, 3)).max(3).max(1)
proto_image = things_pb2.Image(
width=width,
height=height,
image_data=image_data.tobytes())
proto_detections = map(detection_to_proto, detections)
message = things_pb2.ProcessedImage(
image=proto_image,
apriltag_detections=proto_detections)
stream.seek(0)
yield message
except KeyboardInterrupt:
print('interrupted!')
except Exception as e:
print(e)
raise e
finally:
print('ended')
class AtLocalization:
"""
Handles image rectification and april tag detection.
"""
def __init__(self, camera_info, tag_size):
# init image rectification
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(camera_info)
self.K_rect, self.mapx, self.mapy = self._init_rectification()
# init apriltag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.tag_size = tag_size
self.camera_params = (self.K_rect[0, 0], self.K_rect[1, 1],
self.K_rect[0, 2], self.K_rect[1, 2])
def _init_rectification(self):
"""
Establish rectification mapping.
"""
w = self.pcm.width
h = self.pcm.height
K_rect, roi = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D,
(w, h), 1.0)
mapx = np.ndarray(shape=(h, w, 1), dtype='float32')
mapy = np.ndarray(shape=(h, w, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(w, h), cv2.CV_32FC1, mapx,
mapy)
return K_rect, mapx, mapy
def rectify(self, img_raw, interpolation=cv2.INTER_NEAREST):
"""
Rectify image.
"""
return cv2.remap(img_raw, self.mapx, self.mapy, interpolation)
def detect(self, img):
img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(img_gray,
estimate_tag_pose=True,
camera_params=self.camera_params,
tag_size=self.tag_size)
return tags
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
self.bridge = CvBridge()
self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0,
quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0)
# subscribe to camera stream
self.sub_camera_img = rospy.Subscriber("camera_node/image/compressed", CompressedImage, self.callback,
queue_size=1)
# publish modified image
self.pub_modified_img = rospy.Publisher(f"~image/compressed", CompressedImage,
queue_size=1)
calibration_data = self.read_yaml_file(f"/data/config/calibrations/camera_intrinsic/{self.veh}.yaml")
self.K = np.array(calibration_data["camera_matrix"]["data"]).reshape(3, 3)
self.log("Letsgoooooo")
def callback(self, msg):
"""
On camera callback, project the little duckies into the image on top of the april tags.
"""
img = self.read_image(msg) # convert to cv2 img
# detect april tag and extract its reference frame
grayscale_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(grayscale_img, estimate_tag_pose=False, camera_params=None, tag_size=None)
# self.visualize_at_detection(img, tags)
# determine H (using apriltag library)
homography_list = [tag.homography for tag in tags]
# derive P
p_list = [self.projection_matrix(self.K, H) for H in homography_list]
# project the model and draw it (using Renderer)
for P in p_list:
img = self.renderer.render(img, P)
# publish modified image
img_out = self.bridge.cv2_to_compressed_imgmsg(img)
img_out.header = msg.header
img_out.format = msg.format
self.pub_modified_img.publish(img_out)
@staticmethod
def projection_matrix(K, H):
"""
Find the projection matrix by expanding H.
"""
R_2d = np.linalg.inv(K) @ H # R_2d = [r1 r2 t]
R_2d = R_2d / np.linalg.norm(R_2d[:, 0])
r1 = R_2d[:, 0]
r2 = R_2d[:, 1]
t = R_2d[:, 2]
r3 = np.cross(r1, r2)
R = np.column_stack((r1, r2, r3)) # R = [r1 r2 r3]
# make sure R is orthogonal
W, U, Vt = cv2.SVDecomp(R)
R = U @ Vt
Rt = np.column_stack((R, t)) # Rt = [r1 r2 r3 t]
P = K @ Rt
return P
def read_image(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def read_yaml_file(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
@staticmethod
def visualize_at_detection(img, tags):
"""
Visualize detected april tags for debugging.
"""
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(img, tuple(tag.corners[idx - 1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)),
(0, 255, 0))
cv2.putText(img, str(tag.tag_id),
org=(tag.corners[0, 0].astype(int) + 10, tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
def on_shutdown(self):
super(ARNode, self).on_shutdown()
class FusedLocalizationNode(DTROS):
def __init__(self, node_name):
super(FusedLocalizationNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(
f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100)
self.rectify_flag = rospy.get_param(
f'/{self.veh_name}/{node_name}/rectify', 100)
self.encoder_conf_flag = False
self.bridge = CvBridge()
self.image = None
self.image_timestamp = rospy.Time.now()
self.cam_info = None
self.camera_info_received = False
self.newCameraMatrix = None
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
trans_base_cam = np.array([0.0582, 0.0, 0.1072])
rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]),
np.radians(15))
rot_cam_base = rot_base_cam.T
trans_cam_base = -rot_cam_base @ trans_base_cam
self.pose_cam_base = SE3_from_rotation_translation(
rot_cam_base, trans_cam_base)
self.tfs_msg_cam_base = TransformStamped()
self.tfs_msg_cam_base.header.frame_id = 'camera'
self.tfs_msg_cam_base.header.stamp = rospy.Time.now()
self.tfs_msg_cam_base.child_frame_id = 'at_baselink'
self.tfs_msg_cam_base.transform = self.pose2transform(
self.pose_cam_base)
self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster()
translation_map_at = np.array([0.0, 0.0, 0.08])
rot_map_at = np.eye(3)
self.pose_map_at = SE3_from_rotation_translation(
rot_map_at, translation_map_at)
self.tfs_msg_map_april = TransformStamped()
self.tfs_msg_map_april.header.frame_id = 'map'
self.tfs_msg_map_april.header.stamp = rospy.Time.now()
self.tfs_msg_map_april.child_frame_id = 'apriltag'
self.tfs_msg_map_april.transform = self.pose2transform(
self.pose_map_at)
self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster()
self.tfs_msg_april_cam = TransformStamped()
self.tfs_msg_april_cam.header.frame_id = 'apriltag'
self.tfs_msg_april_cam.header.stamp = rospy.Time.now()
self.tfs_msg_april_cam.child_frame_id = 'camera'
self.br_april_cam = tf.TransformBroadcaster()
self.tfs_msg_map_base = TransformStamped()
self.tfs_msg_map_base.header.frame_id = 'map'
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.tfs_msg_map_base.child_frame_id = 'fused_baselink'
self.br_map_base = tf.TransformBroadcaster()
self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi])
R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2)
R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2)
R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_dtc_c = R_x_c @ R_z_c
R_a_dta = R_y_a @ R_z_a
self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c,
np.array([0, 0, 0]))
self.T_a_dta = SE3_from_rotation_translation(R_a_dta,
np.array([0, 0, 0]))
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_cam_info = rospy.Subscriber(
f'/{self.veh_name}/camera_node/camera_info',
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.pub_tf_fused_loc = rospy.Publisher(
f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped,
queue_size=10)
def image_cb(self, image_msg):
try:
image = self.readImage(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
self.image = image
self.image_timestamp = image_msg.header.stamp
def cb_camera_info(self, msg):
if not self.camera_info_received:
self.cam_info = msg
self.camera_info_received = True
def detect_april_tag(self):
if self.rectify_flag:
K = self.newCameraMatrix
else:
K = np.array(self.cam_info.K).reshape((3, 3))
camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
img_grey = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(img_grey, True, camera_params, 0.065)
list_pose_tag = []
for tag in tags:
pose_SE3 = SE3_from_rotation_translation(tag.pose_R,
np.squeeze(tag.pose_t))
list_pose_tag.append(pose_SE3)
return list_pose_tag
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def pose2transform(self, pose: SE3) -> Transform:
rot, trans = rotation_translation_from_SE3(pose)
quat_tf = quaternion_from_matrix(pose)
quaternion = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
return Transform(translation, quaternion)
def pose_inverse(self, pose: SE3) -> Transform:
rot, trans = rotation_translation_from_SE3(pose)
rot_inv = rot.T
trans_inv = -rot_inv @ trans
return SE3_from_rotation_translation(rot_inv, trans_inv)
def rectify_image(self):
cameraMatrix = np.array(self.cam_info.K).reshape((3, 3))
distCoeffs = np.array(self.cam_info.D)
newCameraMatrix, roi = cv2.getOptimalNewCameraMatrix(
cameraMatrix, distCoeffs, (640, 480), 1.0)
self.newCameraMatrix = newCameraMatrix
mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix, distCoeffs,
np.eye(3), newCameraMatrix,
(640, 480), cv2.CV_32FC1)
self.image = cv2.remap(self.image, mapx, mapy, cv2.INTER_LINEAR)
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
if self.rectify_flag:
self.rectify_image()
list_pose_tag = self.detect_april_tag()
if not list_pose_tag:
if self.encoder_conf_flag:
trans_map_base_2D, theta_map_base_2D = translation_angle_from_SE2(
self.pose_map_base_SE2)
self.pose_map_base_SE2 = encoder_localization_client(
trans_map_base_2D[0], trans_map_base_2D[1],
theta_map_base_2D)
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
else:
pose_dta_dtc = self.pose_inverse(list_pose_tag[0])
pose_april_cam = self.T_a_dta @ pose_dta_dtc @ self.T_dtc_c
pose_map_base = self.pose_map_at @ pose_april_cam @ self.pose_cam_base
quat_map_base = quaternion_from_matrix(pose_map_base)
euler = euler_from_quaternion(quat_map_base)
theta = euler[2]
rot_map_base, translation_map_base = rotation_translation_from_SE3(
pose_map_base)
pose_map_base_SE2_enc = encoder_localization_client(
translation_map_base[0], translation_map_base[1], theta)
self.encoder_conf_flag = True
self.pose_map_base_SE2 = SE2_from_xytheta(
[translation_map_base[0], translation_map_base[1], theta])
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
self.tfs_msg_map_april.header.stamp = self.image_timestamp
self.static_tf_br_map_april.sendTransform(
self.tfs_msg_map_april)
self.tfs_msg_cam_base.header.stamp = self.image_timestamp
self.static_tf_br_cam_base.sendTransform(self.tfs_msg_cam_base)
self.tfs_msg_april_cam.transform = self.pose2transform(
pose_april_cam)
self.tfs_msg_april_cam.header.stamp = self.image_timestamp
self.br_april_cam.sendTransformMessage(self.tfs_msg_april_cam)
# grab the current frame
frame = vs.read()
# handle the frame from VideoCapture or VideoStream
frame = frame[1] if args.get("video", False) else frame
# if we are viewing a video and we did not grab a frame,
# then we have reached the end of the video
if frame is None:
break
# (assuming you have a variable "frame" with the current camera frame in it)
frameGray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #converts to grayscale
tags = at_detector.detect(frameGray,
estimate_tag_pose=True,
camera_params=picam_params,
tag_size=tag_size)
# detector requires a grayscale frame; do not estimate pose for now
for tag in tags:
# print the detected tag id and floating point coordinates of its center
# print("%s: %6.1f,%6.1f" % (tag.tag_id, tag.center[0], tag.center[1]))
# draw a point at the center of the apriltag
center = tuple(tag.center.astype(int).ravel())
# cv2.circle(frame, center, 5, (0, 255, 0), -1)
# draw box around apriltag using corners
for i in range(4):
pt0 = tuple(tag.corners[i - 1].astype(int).ravel())
pt1 = tuple(tag.corners[i].astype(int).ravel())
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
# Load calibration files
self.calib_data = self.readYamlFile(
'/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml')
self.log('Loaded intrinsics calibration file')
self.extrinsics = self.readYamlFile(
'/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml')
self.log('Loaded extrinsics calibration file')
# Retrieve intrinsic info
self.cam_info = self.setCamInfo(self.calib_data)
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
self.log('Renderer object created')
# Create AprilTag detector object
self.at_detector = Detector()
# Create cv_bridge
self.bridge = CvBridge()
# Define subscriber to recieve images
self.image_sub = rospy.Subscriber(
'/' + self.veh + '/camera_node/image/compressed', CompressedImage,
self.callback)
# Publish the rendered image to a new topic
self.augmented_pub = rospy.Publisher('~image/compressed',
CompressedImage,
queue_size=1)
self.log(node_name + ' initialized and running')
def callback(self, ros_image):
'''
MAIN TASK
Converse the image from the camera_node to a cv2 image. Then, detects
the apriltags position and retrieves the homography. With the homography
and the intrinsic camera matrix, it computes the projection matrix. Finally,
the provided Renderer class returns an image with the model rendered on it,
which is then published to a new topic
'''
# Convert to cv2 image
image = self.readImage(ros_image)
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Extract camera parameters
K = np.array(self.cam_info.K).reshape((3, 3))
cam_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
# Detect apriltags
detections = self.at_detector.detect(gray_image,
estimate_tag_pose=True,
camera_params=cam_params,
tag_size=0.065)
# Render a model in each of the detected Apriltags
rendered_image = image
for tag in detections:
# Extract homography
H = np.array(tag.homography).reshape((3, 3))
# Obtain Projection matrix
projection_matrix = self.projection_matrix(K, H)
# Render the model
rendered_image = self.renderer.render(rendered_image,
projection_matrix)
# Publish image with models rendered
augmented_image = self.bridge.cv2_to_compressed_imgmsg(rendered_image)
self.augmented_pub.publish(augmented_image)
def setCamInfo(self, calib_data):
'''
Introduces the camera information from the dictionary
obtained reading the yaml file to a CameraInfo object
'''
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def projection_matrix(self, intrinsic, homography):
"""
Method that computes a Projection matrix from 3D world to image plane.
Args:
intrinsic: (np.array) Camera matrix
homography: (np.array) 2D Homography obtained by detecting the apriltag position
Returns:
P: Projeciton matrix
"""
# Extract parameters
K = intrinsic
K_inv = np.linalg.inv(K)
H = homography
# Compute 2D Rt matrix
Rt = K_inv @ H
# Normalize so that ||R1|| = 1
Rt = Rt / np.linalg.norm(Rt[:, 0])
t = np.array(Rt[:, 2]).reshape((3, 1))
# Extract rotation vectors
R1 = np.array(Rt[:, 0]).reshape((3, 1))
R2 = np.array(Rt[:, 1]).reshape((3, 1))
# Compute third rotation vector to be orthogonal to the other two
R3 = np.array(np.cross(Rt[:, 0], Rt[:, 1])).reshape((3, 1))
# Construct 3D rotation matrix
R = np.concatenate((R1, R2, R3), axis=1)
# Perform polar decomposition to enforce orthogonality
U, S, Vt = np.linalg.svd(R)
R = U @ Vt
# Compute projection matrix
P = K @ (np.concatenate((R, t), axis=1))
return P
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
class AprilTag_Localization(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(AprilTag_Localization, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
# read calibration
intrinsic_fname = '/data/config/calibrations/camera_intrinsic/default.yaml'
if 'INTRINSICS_FILE' in os.environ:
intrinsic_fname = os.environ['CALIBRATION_FILE']
intrinsics = self.readYamlFile(intrinsic_fname)
self.D = np.array(intrinsics['distortion_coefficients']['data'])
self.K = np.reshape(np.array(intrinsics['camera_matrix']['data']),
[3, 3])
# init poses
self.x = 0
self.y = 0
self.theta = 0
# transforms
self.A_to_map = tf.transformations.identity_matrix() # april-tag
self.A_to_map[2, 3] = -0.095
self.Ad_to_A = np.array([[0, 1, 0, 0], [0, 0, -1, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.C_to_Cd = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 0, 1]])
self.baselink_to_camera = tf.transformations.rotation_matrix(
np.pi * (20 / 180), (0, 1, 0))
self.baselink_to_camera[0:3, 3] = np.array([0.0582, 0, 0.1072])
# april tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# subscriber for images
self.listener = tf.TransformListener()
self.sub = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed', CompressedImage,
self.image_callback)
# publisher
self.pub_pose = rospy.Publisher(f'/{self.veh_name}/pose/apriltag',
TransformStamped,
queue_size=30)
self.br = tf.TransformBroadcaster()
# other important things
self.cvbr = CvBridge()
self.log("Initialized.")
def image_callback(self, msg):
"""Detects april-tags and renders duckie on them."""
img = self.readImage(msg)
stamp = rospy.Time(msg.header.stamp.secs, msg.header.stamp.nsecs)
if len(img) > 0:
K = self.K
tags = self.at_detector.detect(
img[:, :, 0],
estimate_tag_pose=True,
camera_params=[K[0, 0], K[1, 1], K[0, 2], K[1, 2]],
tag_size=0.0675)
### TODO: Code here to calculate pose of camera (?)
self.broadcast_pose(self.baselink_to_camera, 'at_baselink',
'camera', stamp)
if len(tags) > 0:
at_camera = tf.transformations.identity_matrix()
at_camera[0:3, 0:3] = np.array(
tags[0].pose_R) ## only detects first tag!
at_camera[0:3, 3] = np.array(tags[0].pose_t).flatten()
at = self.C_to_Cd @ at_camera @ self.Ad_to_A
self.broadcast_pose(at, 'camera', 'apriltag', rospy.Time.now())
self.broadcast_pose(self.A_to_map, 'apriltag', 'map',
rospy.Time.now())
try:
(trans, q) = self.listener.lookupTransform(
'map', 'at_baselink', rospy.Time(0))
msg = TransformStamped()
msg.header.frame_id = 'map'
msg.child_frame_id = 'at_baselink'
msg.transform.translation.x = trans[0]
msg.transform.translation.y = trans[1]
msg.transform.translation.z = trans[2]
msg.transform.rotation.x = q[0]
msg.transform.rotation.y = q[1]
msg.transform.rotation.z = q[2]
msg.transform.rotation.w = q[3]
msg.header.stamp = rospy.Time.now()
self.pub_pose.publish(msg)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
pass
def broadcast_pose(self, pose, frame_id, child_frame_id, stamp):
q = tf.transformations.quaternion_from_matrix(pose)
self.br.sendTransform((pose[0, 3], pose[1, 3], pose[2, 3]), q, stamp,
child_frame_id, frame_id)
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.cvbr.compressed_imgmsg_to_cv2(msg_image)
image_undistorted = cv2.undistort(
cv_image, self.K, self.D) # TODO: move this to GPU.
return image_undistorted
except CvBridgeError as e:
self.log(e)
return []
except AttributeError as e:
return []
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(AprilTag_Localization, self).onShutdown()
class ATLocalizationNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATLocalizationNode, self).__init__(
node_name=node_name, node_type=NodeType.PERCEPTION)
self.veh = rospy.get_namespace().strip("/")
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber(
'camera_node/image/compressed', CompressedImage, self.callback, queue_size=1, buff_size=(20*(1024**2)))
# self.debug_pub = rospy.Publisher(
# '~debug_image/compressed', CompressedImage)
# tf broadcasters
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
self.tf_br = tf2_ros.TransformBroadcaster()
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# keep track of the ID of the landmark tag
self.at_id = None
# get camera calibration parameters (homography, camera matrix, distortion parameters)
intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
extrinsics_file = '/data/config/calibrations/camera_extrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
intrinsics_file))
rospy.loginfo('Reading camera extrinsics from {}'.format(
extrinsics_file))
intrinsics = readYamlFile(intrinsics_file)
extrinsics = readYamlFile(extrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
cam_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
distortion_coeff = np.array(
intrinsics['distortion_coefficients']['data'])
H_ground2img = np.linalg.inv(
np.array(extrinsics['homography']).reshape(3, 3))
# precompute some quantities
new_cam_mat, _ = cv2.getOptimalNewCameraMatrix(
cam_mat, distortion_coeff, (640, 480), 1.0)
self.map1, self.map2, = cv2.initUndistortRectifyMap(
cam_mat, distortion_coeff, np.eye(3), new_cam_mat, (640, 480), cv2.CV_32FC1)
self.camera_params = (
new_cam_mat[0, 0], new_cam_mat[1, 1], new_cam_mat[0, 2], new_cam_mat[1, 2])
# define and broadcast static tfs
self.camloc_camcv = np.array([[0.0, 0.0, 1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.atcv_atloc = np.array([[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
camcv_base_estimated = estimateTfFromHomography(
H_ground2img, new_cam_mat)
theta = 15.0 / 180.0 * np.pi
base_camloc_nominal = np.array([[np.cos(theta), 0.0, np.sin(theta), 0.0582],
[0.0, 1.0, 0.0, 0.0],
[-np.sin(theta), 0.0,
np.cos(theta), 0.1072],
[0.0, 0.0, 0.0, 1.0]])
self.camloc_base = self.camloc_camcv @ camcv_base_estimated
# self.camloc_base = np.linalg.inv(base_camloc_nominal)
self.map_atloc = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.085],
[0.0, 0.0, 0.0, 1.0]])
broadcastTF(self.map_atloc, 'map', 'april_tag', self.static_tf_br)
def callback(self, data):
img_gray = cv2.cvtColor(self.readImage(data), cv2.COLOR_BGR2GRAY)
try:
undistorted_img = cv2.remap(
img_gray, self.map1, self.map2, cv2.INTER_LINEAR)
# msg = self.bridge.cv2_to_compressed_imgmsg(undistorted_img, dst_format='jpeg')
# self.debug_pub.publish(msg)
except:
rospy.logwarn(
'Was not able to undistort image for april tag detection')
return
tags = self.at_detector.detect(
undistorted_img, estimate_tag_pose=True, camera_params=self.camera_params, tag_size=TAG_SIZE)
for tag in tags:
if self.at_id is None:
self.at_id = tag.tag_id
if tag.tag_id == self.at_id: # Assumes all tags have a unique id
camcv_atcv = np.zeros((4, 4))
camcv_atcv[:3, :3] = tag.pose_R
camcv_atcv[:3, -1] = np.squeeze(tag.pose_t)
camcv_atcv[-1, -1] = 1.0
camloc_atloc = self.camloc_camcv @ camcv_atcv @ self.atcv_atloc
atloc_camloc = np.linalg.inv(camloc_atloc)
map_base = self.map_atloc @ atloc_camloc @ self.camloc_base
broadcastTF(atloc_camloc, 'april_tag', 'camera',
self.tf_br, data.header.stamp)
broadcastTF(map_base, 'map', 'at_baselink',
self.tf_br, data.header.stamp)
break
def readImage(self, msg_image):
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
# grab an image from the camera
file = camera.getFileName()
imageBW = camera.getImage()
# we're out of images
if imageBW is None:
break
# AprilDetect, after accounting for distortion (if fisheye)
if camParams['fisheye']:
dim1 = imageBW.shape[:2][::-1] #dim1 is the dimension of input image to un-distort
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, numpy.eye(3), K, dim1, cv2.CV_16SC2)
undistorted_img = cv2.remap(imageBW, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
tags = at_detector.detect(undistorted_img, True, camParams['cam_params'], args.tagSize/1000)
else:
tags = at_detector.detect(imageBW, True, camParams['cam_params'], args.tagSize/1000)
if file:
print("File: {0}".format(file))
# get time to capture and convert
print("Time to capture and detect = {0:.3f} sec, found {1} tags".format(time.time() - myStart, len(tags)))
for tag in tags:
tagpos = getPos(getTransform(tag))
tagrot = getRotation(getTransform(tag))
print("Tag {0} pos = {1} m, Rot = {2} deg".format(tag.tag_id, tagpos.round(3), tagrot.round(1)))
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
#
# Write your code here
#
self.bridge = CvBridge()
self.image = None
self.image_height = None
self.image_width = None
self.cam_info = None
self.camera_info_received = False
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1
)
self.sub_cam_info = rospy.Subscriber(f'/{self.veh}/camera_node/camera_info', CameraInfo,
self.cb_camera_info, queue_size=1)
self.pub_aug_image = rospy.Publisher(f'/{self.veh}/{node_name}/image/compressed',
CompressedImage, queue_size=10)
def image_cb(self, image_msg):
try:
image = self.readImage(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
self.image = image
self.image_height = np.shape(image)[0]
self.image_width = np.shape(image)[1]
def cb_camera_info(self, msg):
if not self.camera_info_received:
self.cam_info = msg
self.camera_info_received = True
def detect_april_tag(self):
K = np.array(self.cam_info.K).reshape((3, 3))
camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
img_grey = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(img_grey, True, camera_params, 0.065)
list_hom_tag = []
for tag in tags:
list_hom_tag.append(tag.homography)
return list_hom_tag
def projection_matrix(self, intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
K = intrinsic
P_ext = np.linalg.inv(K) @ homography
r_1 = P_ext[:, 0]
r_1_norm = np.linalg.norm(r_1)
r_1 = r_1 / r_1_norm
r_2 = P_ext[:, 1]
r_2_norm = np.linalg.norm(r_2)
r_2 = r_2 / r_2_norm
r_3 = np.cross(r_1, r_2)
t_norm = (r_1_norm + r_2_norm)/2
t = P_ext[:, 2] / t_norm
P_ext_new = np.concatenate((r_1, r_2, r_3, t)).reshape((-1, 4), order='F')
P = K @ P_ext_new
return P
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self,fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
list_hom_tag = self.detect_april_tag()
if not list_hom_tag:
continue
img_rend = self.image
for h in list_hom_tag:
proj_mat = self.projection_matrix(np.array(self.cam_info.K).reshape((3, 3)), h)
img_rend = self.renderer.render(img_rend, proj_mat)
comp_image = self.bridge.cv2_to_compressed_imgmsg(img_rend)
self.pub_aug_image.publish(comp_image)
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber('camera_node/image/compressed',
CompressedImage, self.callback)
# construct publisher for AR images
self.pub = rospy.Publisher('~augemented_image/compressed',
CompressedImage,
queue_size=10)
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=2.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# get camera calibration parameters (homography, camera matrix, distortion parameters)
self.intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
self.intrinsics_file))
intrinsics = self.readYamlFile(self.intrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
self.camera_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
# Precompute some matricies
self.camera_params = (self.camera_mat[0, 0], self.camera_mat[1, 1],
self.camera_mat[0, 2], self.camera_mat[1, 2])
self.inv_camera_mat = np.linalg.inv(self.camera_mat)
self.mat_3Dto2D = np.concatenate((np.identity(3), np.zeros((3, 1))),
axis=1)
self.T = np.zeros((4, 4))
self.T[-1, -1] = 1.0
def callback(self, data):
img = self.readImage(data)
tags = self.at_detector.detect(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY),
estimate_tag_pose=True,
camera_params=self.camera_params,
tag_size=TAG_SIZE)
for tag in tags:
H_tag2img = tag.homography
projection_mat = self.projection_matrix(H_tag2img)
self.renderer.render(img, projection_mat)
msg = self.bridge.cv2_to_compressed_imgmsg(img, dst_format='jpeg')
self.pub.publish(msg)
def projection_matrix(self, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
T_plane = self.inv_camera_mat @ homography
T_plane = T_plane / np.linalg.norm(
T_plane[:, 0]
) # estimate scale using rotation matrix basis constraint
r1 = T_plane[:, 0]
r2 = T_plane[:, 1]
t = T_plane[:, 2]
# Make sure r1 and r2 form an orthogonal basis then generate r3
r2 = (r2 - np.dot(r2, r1) * r1)
r2 = r2 / np.linalg.norm(r2)
r3 = np.cross(r1, r2)
self.T[:3, :] = np.column_stack((r1, r2, r3, t))
return self.camera_mat @ self.mat_3Dto2D @ self.T
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
class AprilTagDetector(DTROS):
def __init__(self):
super(AprilTagDetector,
self).__init__(node_name='apriltag_detector_node',
node_type=NodeType.PERCEPTION)
# get static parameters
self.family = rospy.get_param('~family', 'tag36h11')
self.nthreads = rospy.get_param('~nthreads', 1)
self.quad_decimate = rospy.get_param('~quad_decimate', 1.0)
self.quad_sigma = rospy.get_param('~quad_sigma', 0.0)
self.refine_edges = rospy.get_param('~refine_edges', 1)
self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25)
self.tag_size = rospy.get_param('~tag_size', 0.065)
# dynamic parameter
self.detection_freq = DTParam('~detection_freq',
default=-1,
param_type=ParamType.INT,
min_value=-1,
max_value=30)
self._detection_reminder = DTReminder(
frequency=self.detection_freq.value)
# camera info
self._camera_parameters = None
self._camera_frame = None
# create detector object
self._at_detector = Detector(families=self.family,
nthreads=self.nthreads,
quad_decimate=self.quad_decimate,
quad_sigma=self.quad_sigma,
refine_edges=self.refine_edges,
decode_sharpening=self.decode_sharpening)
# create a CV bridge object
self._bridge = CvBridge()
# create subscribers
self._img_sub = rospy.Subscriber('image_rect', Image, self._img_cb)
self._cinfo_sub = rospy.Subscriber('camera_info', CameraInfo,
self._cinfo_cb)
# create publishers
self._img_pub = rospy.Publisher('tag_detections_image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION)
self._tag_pub = rospy.Publisher('tag_detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self._img_pub_busy = False
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()
# spin forever
rospy.spin()
def _img_cb(self, data):
if self._camera_parameters is None:
return
if not self._detection_reminder.is_time(
frequency=self.detection_freq.value):
return
# ---
# turn image message into grayscale image
img = self._bridge.imgmsg_to_cv2(data, desired_encoding='mono8')
# detect tags
tags = self._at_detector.detect(img, True, self._camera_parameters,
self.tag_size)
# draw the detections on an image (if needed)
if self._img_pub.anybody_listening() and not self._img_pub_busy:
self._img_pub_busy = True
Thread(target=self._publish_detections_image,
args=(img, tags)).start()
# pack detections into a message
msg = AprilTagDetectionArray()
detection_time = rospy.Time.now()
msg.header.stamp = detection_time
msg.header.frame_id = self._camera_frame
for tag in tags:
# turn rotation matrix into quaternion
q = _matrix_to_quaternion(tag.pose_R)
p = tag.pose_t.T[0]
# create single tag detection object
detection = AprilTagDetection(
transform=Transform(translation=Vector3(x=p[0], y=p[1],
z=p[2]),
rotation=Quaternion(x=q[0],
y=q[1],
z=q[2],
w=q[3])),
tag_id=tag.tag_id,
tag_family=str(tag.tag_family),
hamming=tag.hamming,
decision_margin=tag.decision_margin,
homography=tag.homography.flatten().astype(
np.float32).tolist(),
center=tag.center.tolist(),
corners=tag.corners.flatten().tolist(),
pose_error=tag.pose_err)
# add detection to array
msg.detections.append(detection)
# publish tf
self._tf_bcaster.sendTransform(p.tolist(), q.tolist(),
detection_time,
'/tag{:s}'.format(str(tag.tag_id)),
self._camera_frame)
# publish detections
self._tag_pub.publish(msg)
# update healthy frequency metadata
self._tag_pub.set_healthy_freq(self._img_sub.get_frequency())
self._img_pub.set_healthy_freq(self._img_sub.get_frequency())
def _publish_detections_image(self, img, tags):
# get a color buffer from the BW image
color_img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
# draw each tag
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(color_img, tuple(tag.corners[idx - 1, :].astype(int)),
tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
# draw the tag ID
cv2.putText(color_img,
str(tag.tag_id),
org=(tag.corners[0, 0].astype(int) + 10,
tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
# pack image into a message
img_msg = self._bridge.cv2_to_compressed_imgmsg(color_img)
# ---
self._img_pub.publish(img_msg)
self._img_pub_busy = False
def _cinfo_cb(self, data):
K = np.array(data.K).reshape((3, 3))
self._camera_parameters = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
self._camera_frame = data.header.frame_id
# once we got the camera info, we can stop the subscriber
self._cinfo_sub.shutdown()
# by frame
ret, frame = vid.read()
grayFrame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
curr_detections = at_detector.detect(grayFrame,
estimate_tag_pose=True,
camera_params=[1402, 1402, 642, 470],
tag_size=0.091)
for i in range(len(curr_detections)):
detections[curr_detections[i].tag_id] = curr_detections[i]
print(i)
blur = cv2.GaussianBlur(grayFrame, (5, 5), 0)
ret, bw_im = cv2.threshold(blur, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)
# zbar
qr = decode(bw_im, symbols=[ZBarSymbol.QRCODE])
# print(qr)
for j in range(len(qr)):
class ApriltagDetector:
"""
ApriltagDetector类, 用来检测特定的Apriltag标签
Attributes:
cali_file: str 广角摄像头矫正文件路径，如果想做姿态估计需要用到
"""
def __init__(self):
cur_dir = os.path.dirname(os.path.abspath(__file__))
# self.cali_file = rospkg.RosPack().get_path('pi_cam') + "/camera_info/calibrations/default.yaml"
# self.cali_file = "default.yaml"
self.camera_info_msg = load_camera_info_3()
# self.detector = Detector(
# print(cur_dir + '/../../../../devel_isolated/apriltag/lib')
# self.detector = Detector(searchpath=[cur_dir + '/../../../../devel_isolated/apriltag/lib'],
self.detector = Detector(
# self.detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=3.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.cameraMatrix = np.array(
self.camera_info_msg.K).reshape((3, 3))
self.camera_params = (
self.cameraMatrix[0, 0], self.cameraMatrix[1, 1], self.cameraMatrix[0, 2], self.cameraMatrix[1, 2])
self.image = None
self.detections = []
self.tags = []
self.tag = None
if hasattr(R, 'from_dcm'):
self.from_dcm_or_matrix = R.from_dcm
else:
self.from_dcm_or_matrix = R.from_matrix
def detect(self, cv_image, label_tags=True, tag_size=0.065):
"""
检测函数
Keyword arguments:
cv_image: image 原图
tag_size：float Apriltag标签的尺寸，以米为单位,估算距离需要
Returns:
tags: [Detection object] 检测到的Apriltag
Detection object:
tag_family = tag36h11
tag_id = 5
hamming = 0
decision_margin = 75.3475112915
homography = [[-4.04986020e+00 -8.25442426e+00 7.03246452e+02]
[ 1.60941194e+01 2.64459780e+00 2.71387964e+02]
[-3.94154088e-03 1.31415634e-02 1.00000000e+00]]
center = [703.24645207 271.38796427]
corners = [[687.30065918 253.60606384]
[684.64343262 287.48184204]
[719.746521 289.78796387]
[722.19494629 254.99520874]]
pose_R = [[-0.05079941 -0.99821651 0.03135634]
[ 0.99851052 -0.05139001 -0.01832521]
[ 0.01990393 0.03037872 0.99934027]]
pose_t = [[0.72577546]
[0.04402775]
[0.58391835]]
pose_err = 5.94095039944e-07
"""
if len(cv_image.shape) == 3:
image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
else:
image_gray = cv_image
self.tags = self.detector.detect(
image_gray, True, self.camera_params, tag_size) # tag size in meter
if label_tags:
self.label_tags(cv_image, self.tags)
self.image = cv_image
self.detections = self.toApriltagDetections()
if len(self.detections) > 0:
self.tag = self.detections[0]
else:
self.tag = None
return cv_image
def label_tags(self, rect_image, tags=None):
if tags is None:
tags = self.tags
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(rect_image, tuple(
tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
cv2.putText(rect_image, str(tag.tag_id),
org=(tag.corners[0, 0].astype(int)+10,
tag.corners[0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8, color=(0, 0, 255))
def toApriltagDetections(self, tags=None):
if tags is None:
tags = self.tags
detections = []
for tag in tags:
r = self.from_dcm_or_matrix(np.array(tag.pose_R))
offset = np.array(tag.pose_t)*100
euler = r.as_euler('xyz', degrees=True)
detection = (tag.tag_id, euler, offset)
detections.append(detection)
return detections
def isDetected(self, id=None):
if id is None and len(self.detections) > 0:
return True
else:
for tag in self.detections:
if id == tag[0]:
self.tag = tag
return True
self.tag = None
return False
if camParams['fisheye'] and dim1 is None:
#Only need to get mapping at first frame
dim1 = imageBW.shape[:
2][::
-1] #dim1 is the dimension of input image to un-distort
map1, map2 = cv2.fisheye.initUndistortRectifyMap(
K, D, numpy.eye(3), K, dim1, cv2.CV_16SC2)
if camParams['fisheye']:
imageBW = cv2.remap(imageBW,
map1,
map2,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT)
#tags = at_detector.detect(undistorted_img, True, camParams['cam_params'], args.tagSize/1000)
#else:
tags = at_detector.detect(imageBW, True, camParams['cam_params'],
args.tagSize / 1000)
# add any new tags to database, or existing one to duplicates
tagsused = 0
for tag in tags:
if tag.pose_err < args.maxerror * 1e-8:
tagsused += 1
tagPlacement.addTag(tag)
tagPlacement.getBestTransform()
if file:
print("File: {0}".format(file))
#get current location and rotation state of vehicle in ArduPilot NED format (rel camera)
(posD, rotD) = tagPlacement.getArduPilotNED()
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.node_name = rospy.get_name().strip("/")
# initialize renderer
rospack = rospkg.RosPack()
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# initialize apriltag detector
self.at_detector = Detector(
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0,
#searchpath=[]
)
self.at_camera_params = None
# self.at_tag_size = 0.065 # fixed param
self.at_tag_size = 2 # to scale pose matrix to homography
# initialize member variables
self.bridge = CvBridge()
self.camera_info_received = False
self.camera_info = None
self.camera_P = None
self.camera_K = None
self.cv2_img = None
# initialize subs and pubs
self.sub_compressed_img = Subscriber(
"/{}/camera_node/image/compressed".format(self.veh_name),
CompressedImage,
self.cb_image,
queue_size=1)
self.loginfo("Subcribing to topic {}".format(
"/{}/camera_node/image/compressed".format(self.veh_name)))
self.sub_camera_info = Subscriber("/{}/camera_node/camera_info".format(
self.veh_name),
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.loginfo("Subcribing to topic {}".format(
"/{}/camera_node/camera_info".format(self.veh_name)))
self.pub_aug_img = Publisher(
"/{}/{}/augmented_image/compressed".format(self.veh_name,
self.node_name),
CompressedImage,
queue_size=1)
self.loginfo("Publishing to topic {}".format(
"/{}/{}/augmented_image/compressed".format(self.veh_name,
self.node_name)))
def cb_image(self, msg):
# cv2_img = self.bridge.compressed_imgmsg_to_cv2(msg)
self.img_msg = msg
return
def aug_image_and_pub(self, msg):
cv2_img = self.bridge.compressed_imgmsg_to_cv2(msg)
# TODO: 1. Detect the AprilTag and extract its reference frame
greyscale_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(greyscale_img, True,
self.at_camera_params, self.at_tag_size)
# TODO: 2. Estimate the homography matrix
# Read function estimate_pose_for_tag_homography in https://github.com/AprilRobotics/apriltag/blob/master/apriltag_pose.c
# The pose_R and pose_t have been already computed
pass
# TODO 3. Derive the transformation from the reference frame of the AprilTag to the target image reference frame
# project from X_T in frame T to pixel: P_camera @ [R|t] @ X_T
# return P_T = P_camera @ [R|t]
def compose_P_from_tag(P_camera, tag):
T = np.concatenate((np.concatenate(
(tag.pose_R, tag.pose_t), axis=1), np.array([[0, 0, 0, 1.0]])),
axis=0)
self.logdebug("P_camera is \n {}".format(P_camera))
P = P_camera @ T
self.logdebug("P is \n {}".format(P))
# norm_P = P/P[2,2]
# self.logdebug("norm_P is \n {}".format(norm_P))
return P
# TODO 4. Project our 3D model in the image (pixel space) and draw it
aug_img = cv2_img
for tag in tags:
aug_img = self.renderer.render(
aug_img, compose_P_from_tag(self.camera_P, tag))
aug_image_msg = self.bridge.cv2_to_compressed_imgmsg(aug_img)
aug_image_msg.header = msg.header
self.pub_aug_img.publish(aug_image_msg)
def cb_camera_info(self, msg):
# self.logdebug("camera info received! ")
if not self.camera_info_received:
self.camera_info = msg
# TODO: decide whether to use K or P for projection
self.camera_P = np.array(msg.P).reshape((3, 4))
self.camera_K = np.array(msg.K).reshape((3, 3))
self.at_camera_params = (self.camera_P[0, 0], self.camera_P[1, 1],
self.camera_P[0, 2], self.camera_P[1, 2])
self.camera_info_received = True
return
# def projection_matrix(self, intrinsic, homography):
# """
# Write here the compuatation for the projection matrix, namely the matrix
# that maps the camera reference frame to the AprilTag reference frame.
# """
#
# # TODO:
# # Write your code here
# #
# pass
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self.camera_info_received:
img_msg = deepcopy(self.img_msg)
self.aug_image_and_pub(img_msg)
# self.logdebug("published one augmented image...")
else:
self.logwarn("Image received, by initialization not finished")
rate.sleep()
def onShutdown(self):
super(ARNode, self).onShutdown()
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
rospy.loginfo("[ARNode]: Initializing Renderer ...")
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# April Tag Detector
rospy.loginfo("[ARNode]: Initializing Detector ...")
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# CV Bridge
rospy.loginfo("[ARNode]: Initializing CV Bridge ...")
self.bridge = CvBridge()
# Intrinsics
rospy.loginfo("[ARNode]: Loading Camera Intrinsics ...")
if (not os.path.isfile(
f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml')
):
rospy.logwarn(
f'[AugmentedRealityBasics]: Could not find {self.veh}.yaml. Loading default.yaml'
)
camera_intrinsic = self.readYamlFile(
f'/data/config/calibrations/camera_intrinsic/default.yaml')
else:
camera_intrinsic = self.readYamlFile(
f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml')
self.K = np.array(camera_intrinsic['camera_matrix']['data']).reshape(
3, 3)
# Subscribers
rospy.loginfo("[ARNode]: Initializing Subscribers ...")
self.image_subscriber = rospy.Subscriber(
'camera_node/image/compressed', CompressedImage, self.callback)
# Publishers
rospy.loginfo("[ARNode]: Initializing Publishers ...")
self.mod_img_pub = rospy.Publisher('ar_node/image/compressed',
CompressedImage,
queue_size=1)
@staticmethod
def projection_matrix(intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
P_list = []
for H in homography:
R2D_t = np.linalg.inv(intrinsic).dot(H) # [R2D_t] = [r1, r2, t]
R2D_t = R2D_t / np.linalg.norm(R2D_t[:, 0]) #normalize
r1 = R2D_t[:, 0]
r2 = R2D_t[:, 1]
t = R2D_t[:, 2]
# r3 must be orthogonal to r1 and r2
r3 = np.cross(r1, r2)
R3D = np.column_stack((r1, r2, r3))
# since R3D is not a proper rotation matrix yet, we have to orthonormalize it with a polar decomposition: A = RP =
W, U, Vt = cv2.SVDecomp(R3D)
R3D = U.dot(Vt)
R3D_t = np.column_stack((R3D, t)) # R3D_t = [r1, r2, r3, t]
P = intrinsic.dot(R3D_t)
P_list.append(P)
return P_list
def callback(self, imgmsg):
# Convert img message to cv2
img = self.readImage(imgmsg)
# Convert to greyscale
img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect april tags
tags = self.at_detector.detect(img_grey,
estimate_tag_pose=False,
camera_params=None,
tag_size=None)
# self.visualize_at_detection(img, tags) #debug visualization, marks april tags
# Get homographies of april tags
homography_list = [tag.homography for tag in tags]
# Compute P from homographies
P_list = self.projection_matrix(self.K, homography_list)
# Render duckies on april tags
for P in P_list:
img = self.renderer.render(img, P)
# Publish modified image
modified_imgmsg = self.bridge.cv2_to_compressed_imgmsg(img)
modified_imgmsg.header.stamp = rospy.Time.now()
self.mod_img_pub.publish(modified_imgmsg)
return
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
if (not os.path.isfile(fname)):
rospy.logwarn("[ARNode]: Could not find file in %s" % fname)
return
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
@staticmethod
def visualize_at_detection(img, tags):
"""
Visualizes detected april tags for debugging.
"""
for tag in tags:
for index in range(len(tag.corners)):
cv2.line(img, tuple(tag.corners[index - 1, :].astype(int)),
tuple(tag.corners[index, :].astype(int)), (0, 255, 0))
cv2.putText(img,
str(tag.tag_id),
org=(tag.corners[0, 0].astype(int) + 10,
tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
def on_shutdown(self):
super(ARNode, self).on_shutdown()
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
self.at_detector = Detector(families='tag36h11',
nthreads=5,
quad_decimate=2.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.bridge=CvBridge()
self.pub_tagimg=rospy.Publisher('at_detect/image/compressed',CompressedImage, queue_size=1)
self.sub_img = rospy.Subscriber('camera_node/image/compressed', CompressedImage, self.cb_at_detect)
def cb_at_detect(self,msg):
img=self.readImage(msg)
img_gray=cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
camerainfo=self.load_intrinsics()
cameraMatrix = np.array(camerainfo.K).reshape((3,3))
camera_params = (cameraMatrix[0,0],cameraMatrix[1,1],cameraMatrix[0,2],cameraMatrix[1,2])
tags = self.at_detector.detect(img_gray, False, camera_params, 0.065)
for tag in tags:
# R=tag.pose_R
# t=(tag.pose_t).reshape((3,))
# extrinsic=np.eye(4)
# extrinsic[0:3,0:3]=R
# extrinsic[0:3,3]=t
# aux=np.eye(3)
# aux=np.c_[aux,[0,0,0]]
# projection=np.linalg.multi_dot([cameraMatrix,aux,extrinsic])
# rospy.loginfo(extrinsic)
projection1=self.projection_matrix(cameraMatrix,tag.homography)
#rospy.loginfo(projection1)
img=self.renderer.render(img,projection1)
# for idx in range(len(tag.corners)):
# cv2.line(img, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
tag_msg=self.bridge.cv2_to_compressed_imgmsg(img)
self.pub_tagimg.publish(tag_msg)
def projection_matrix(self, camera_matrix, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
# Compute rotation along the x and y axis as well as the translation
#homography = homography * (-1)
rot_and_transl = np.dot(np.linalg.inv(camera_matrix), homography)
col_1 = rot_and_transl[:, 0]
col_2 = rot_and_transl[:, 1]
col_3 = rot_and_transl[:, 2]
# normalise vectors
l=np.linalg.norm(col_1, 2)
rot_1 = col_1 / l
rot_2 = col_2 / l
translation = col_3 / l
# compute the orthonormal basis
rot_3 = np.cross(rot_1, rot_2)
# finally, compute the 3D projection matrix from the model to the current frame
projection = np.stack((rot_1, rot_2, rot_3, translation)).T
#rospy.loginfo(projection)
return np.dot(camera_matrix, projection)
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self,fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
def load_intrinsics(self):
# load intrinsic calibration
cali_file_folder = '/data/config/calibrations/camera_intrinsic/'
cali_file = cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
calib_data=self.readYamlFile(cali_file)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def onShutdown(self):
super(ARNode, self).onShutdown()
class LocalizationNode(DTROS):
def __init__(self, node_name):
"""Wheel Encoder Localization Node
Calculates the pose of the Duckiebot using only AprilTags
"""
# Initialize the DTROS parent class
super(LocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
self.veh_name = rospy.get_namespace().strip("/")
# define initial stuff
self.camera_x = 0.065
self.camera_z = 0.11
self.camera_slant = 19/180*math.pi
self.height_streetsigns = 0.07
# Load calibration files
self._res_w = 640
self._res_h = 480
self.calibration_call()
# define tf stuff
self.br = tf.TransformBroadcaster()
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
# Initialize classes
self.at_detec = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0,
quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0)
self.helper = Helpers(self.current_camera_info)
self.bridge = CvBridge()
# Subscribers
self.cam_image = rospy.Subscriber(f'/{self.veh_name}/camera_node/image/compressed', CompressedImage,
self.callback, queue_size=10)
# Publishers
self.pub_baselink_pose = rospy.Publisher('~at_baselink_pose', TransformStamped, queue_size=1)
# static broadcasters
self.broadcast_static_transform_baselink()
self.log("Initialized")
def callback(self, image):
"""
Callback method that ties everything together
"""
# convert the image to cv2 format
image = self.bridge.compressed_imgmsg_to_cv2(image)
# undistort the image
image = self.helper.process_image(image)
# detect AprilTags
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
tags = self.at_detec.detect(gray, estimate_tag_pose=True, camera_params=self.camera_params, tag_size=0.065)
for tag in tags:
self.broadcast_tag_camera_transform(tag)
def broadcast_tag_camera_transform(self, tag):
"""
compute the transform between AprilTag and camera
"""
camera_to_tag_R = tag.pose_R
camera_to_tag_t = tag.pose_t
# build T matrix
camera_to_tag_T = np.append(camera_to_tag_R, camera_to_tag_t, axis=1)
camera_to_tag_T = np.append(camera_to_tag_T, [[0, 0, 0, 1]], axis=0)
# translate T into the rviz frame convention
T_rviz = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
self.rviz_to_tag_T = np.matmul(T_rviz, camera_to_tag_T)
# translation and rotation arrays
br_translation_array = np.array(self.rviz_to_tag_T[0:3, 3])
br_rotation_array = tf.transformations.quaternion_from_matrix(self.rviz_to_tag_T)
# publish & broadcast
br_transform_msg = self.broadcast_packer("camera", "apriltag", br_translation_array, br_rotation_array)
self.br.sendTransform(br_translation_array,
br_rotation_array,
br_transform_msg.header.stamp,
br_transform_msg.child_frame_id,
br_transform_msg.header.frame_id)
# self.pub_baselink_pose.publish(br_transform_msg)
self.broadcast_static_frame_rotation_transform()
self.broadcast_static_transform_apriltag()
self.broadcast_map_baselink_transform()
def broadcast_map_baselink_transform(self):
# baselink --> map transform tf_source_end
map_baselink_T = np.linalg.inv(self.camera_baselink_T @ self.rviz_to_tag_T @ self.frame_rotation_T @ self.map_tag_T)
# map_baselink_T = self.map_tag_T @ self.rviz_to_tag_T @ self.camera_baselink_T
br_translation_array = np.array(map_baselink_T[0:3, 3])
br_rotation_array = tf.transformations.quaternion_from_matrix(map_baselink_T)
br_transform_msg = self.broadcast_packer("at_baselink", "map", br_translation_array, br_rotation_array)
self.pub_baselink_pose.publish(br_transform_msg)
def broadcast_static_transform_baselink(self):
# camera --> baselink transform
static_translation_array = (self.camera_x, 0, self.camera_z)
rotation_matrix = np.array([[math.cos(self.camera_slant), 0, math.sin(self.camera_slant), 0], [0, 1, 0, 0],
[-math.sin(self.camera_slant), 0, math.cos(self.camera_slant), 0], [0, 0, 0, 1]])
static_rotation_array = tf.transformations.quaternion_from_matrix(rotation_matrix)
self.camera_baselink_T = np.copy(rotation_matrix)
self.camera_baselink_T[0:3, 3] = static_translation_array
static_transform_msg = self.broadcast_packer("at_baselink", "camera", static_translation_array, static_rotation_array)
# broadcast packed message
self.static_tf_br.sendTransform(static_transform_msg)
def broadcast_static_frame_rotation_transform(self):
# change in coordinate frame from ATD convention to rviz convention
static_translation_array = (0, 0, 0)
static_rotation = np.linalg.inv(np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]))
self.frame_rotation_T = np.copy(static_rotation)
self.frame_rotation_T[0:3, 3] = static_translation_array
static_rotation_array = tf.transformations.quaternion_from_matrix(static_rotation)
static_transform_msg = self.broadcast_packer("apriltag", "apriltag_rot", static_translation_array, static_rotation_array)
# broadcast packed message
self.static_tf_br.sendTransform(static_transform_msg)
def broadcast_static_transform_apriltag(self):
# map --> AprilTag transform
static_translation_array = (0, 0, -self.height_streetsigns)
static_rotation_array = tf.transformations.quaternion_from_matrix(np.eye(4))
self.map_tag_T = np.copy(np.eye(4))
self.map_tag_T[0:3, 3] = static_translation_array
static_transform_msg = self.broadcast_packer("apriltag_rot", "map", static_translation_array, static_rotation_array)
# broadcast packed message
self.static_tf_br.sendTransform(static_transform_msg)
def calibration_call(self):
# copied from the camera driver (dt-duckiebot-interface)
# For intrinsic calibration
self.cali_file_folder = '/data/config/calibrations/camera_intrinsic/'
self.frame_id = rospy.get_namespace().strip('/') + '/camera_optical_frame'
self.cali_file = self.cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(self.cali_file):
self.logwarn("Calibration not found: %s.\n Using default instead." % self.cali_file)
self.cali_file = (self.cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(self.cali_file):
rospy.signal_shutdown("Found no calibration file ... aborting")
# Load the calibration file
self.original_camera_info = self.load_camera_info(self.cali_file)
self.original_camera_info.header.frame_id = self.frame_id
self.current_camera_info = copy.deepcopy(self.original_camera_info)
self.update_camera_params()
self.log("Using calibration file: %s" % self.cali_file)
# extrinsic calibration
self.ex_cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
self.ex_cali_file = self.ex_cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
self.ex_cali = self.readYamlFile(self.ex_cali_file)
# define camera parameters for AT detector
self.camera_params = [self.current_camera_info.K[0], self.current_camera_info.K[4], self.current_camera_info.K[2], self.current_camera_info.K[5]]
def update_camera_params(self):
# copied from the camera driver (dt-duckiebot-interface)
""" Update the camera parameters based on the current resolution.
The camera matrix, rectification matrix, and projection matrix depend on
the resolution of the image.
As the calibration has been done at a specific resolution, these matrices need
to be adjusted if a different resolution is being used.
"""
scale_width = float(self._res_w) / self.original_camera_info.width
scale_height = float(self._res_h) / self.original_camera_info.height
scale_matrix = np.ones(9)
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[4] *= scale_height
scale_matrix[5] *= scale_height
# Adjust the camera matrix resolution
self.current_camera_info.height = self._res_h
self.current_camera_info.width = self._res_w
# Adjust the K matrix
self.current_camera_info.K = np.array(self.original_camera_info.K) * scale_matrix
# Adjust the P matrix (done by Rohit)
scale_matrix = np.ones(12)
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[5] *= scale_height
scale_matrix[6] *= scale_height
self.current_camera_info.P = np.array(self.original_camera_info.P) * scale_matrix
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
@staticmethod
def load_camera_info(filename):
# copied from the camera driver (dt-duckiebot-interface)
"""Loads the camera calibration files.
Loads the intrinsic camera calibration.
Args:
filename (:obj:`str`): filename of calibration files.
Returns:
:obj:`CameraInfo`: a CameraInfo message object
"""
with open(filename, 'r') as stream:
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
@staticmethod
def broadcast_packer(header_frame, child_frame, translation_array, rotation_array):
transform_msg = TransformStamped()
transform_msg.header.stamp = rospy.Time.now()
transform_msg.header.frame_id = header_frame
transform_msg.child_frame_id = child_frame
transform_msg.transform.translation = Vector3(*translation_array)
transform_msg.transform.rotation = Quaternion(*rotation_array)
return transform_msg
def onShutdown(self):
super(LocalizationNode, self).onShutdown()
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map, startup_check=False):
super(SpecificWorker, self).__init__(proxy_map)
# Drone
self.state = 'idle'
self.x = 0
self.y = 0
self.pose = {}
self.appleCatched = False
self.treeCatched = False
self.depthX = 180
self.depthY = 180
#AprilTags
self.center = 0
self.tags = {}
self.n_tags = 0
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Image
self.image = []
self.depth = []
self.camera_name = "frontCamera"
self.data = {}
self.depth_array = []
self.color = 0
#Timer
self.times = 0
self.Period = 100
if startup_check:
self.startup_check()
else:
self.timer.timeout.connect(self.compute)
self.timer.start(self.Period)
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
#try:
# self.innermodel = InnerModel(params["InnerModelPath"])
#except:
# traceback.print_exc()
# print("Error reading config params")
return True
def draw_image(self, color_):
color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth)
#Marker is on X: 256 Y:256
# cv.drawMarker(color, (int(color_.width/2), int(color_.height/2)), (0, 255, 0), cv.MARKER_CROSS, 25, 2)
cv.drawMarker(color, (self.depthX, self.depthY), (0, 0, 255), cv.MARKER_CROSS, 25, 2)
cv.drawMarker(color, (self.depthX, self.depthY+150), (0, 0, 255), cv.MARKER_CROSS, 25, 2)
cv.drawMarker(color, (self.depthX+150, self.depthY), (0, 0, 255), cv.MARKER_CROSS, 25, 2)
cv.drawMarker(color, (self.depthX+150, self.depthY+150), (0, 0, 255), cv.MARKER_CROSS, 25, 2)
cv.drawMarker(color, (256, 230), (0, 0, 255), cv.MARKER_CROSS, 25, 2)
plt.imshow(color)
def draw_camera(self, color_, title):
plt.figure(1)
plt.clf()
plt.imshow(color_)
plt.title('Front Camera ')
plt.pause(.00001)
@QtCore.Slot()
def compute(self):
all = self.camerargbdsimple_proxy.getAll(self.camera_name)
self.image = all.image
self.depth = all.depth
self.draw_image(self.image)
# Depth processing
# Max value: 10.0 Min value from apple: between 0.18 and 0.19
if len(self.depth.depth) <= 1048576:
self.depth_array = np.frombuffer(self.depth.depth,dtype=np.float32).reshape(self.depth.height,
self.depth.width)
if self.treeCatched == False:
self.x, self.y, self.color = self.colorDetect(self.image, 36, 0, 0, 86, 255, 255) #green
elif self.state != 'turnleft':
self.x, self.y, self.color = self.colorDetect(self.image, 0, 50, 120, 10, 255, 255) #red
if self.state == 'turnleft':
self.ar_detection(self.image)
try:
self.mainSwitch()
except Ice.Exception as e:
print(e)
def mainSwitch(self):
if self.state == 'idle':
self.idle()
elif self.state == 'movex':
self.movex(True)
elif self.state == 'movey':
self.movey(True)
elif self.state == 'advance':
self.advance(True)
elif self.state == 'reverse':
self.reverse(True)
elif self.state == 'turnleft':
self.turn_left(True)
elif self.state == 'turnright':
self.turn_right(True)
elif self.state == 'pickapple':
self.pick_apple()
elif self.state == 'drop':
self.drop()
elif self.state == 'stop':
self.stop()
elif self.state == 'tag':
self.tag()
elif self.state == 'tree':
self.tree()
else:
self.error()
# =============== Drone Movements ===================
# ===================================================
# PoseType parameters for movements
# x = adv rx: None
# y = width ry: None
# z = height rz: rotate
def moveDummy(self, x_=0, y_=0, z_=0, rz_=0):
self.pose = RoboCompCoppeliaUtils.PoseType(x=x_,y=y_,z=z_,rz=rz_)
headCamera = RoboCompCoppeliaUtils.TargetTypes.HeadCamera
self.coppeliautils_proxy.addOrModifyDummy(headCamera, "Quadricopter_target", self.pose)
#State: idle
def idle(self):
print("[ IDLE ] depth = ", self.depth_array[self.depthX][self.depthY])
if self.depth_array[self.depthX][self.depthY] < 0.3:
self.moveDummy(x_= -0.002)
elif self.depth_array[self.depthX][self.depthY] > 0.6:
self.moveDummy(x_= 0.002)
elif self.depth_array[self.depthX][self.depthY] > 0.3 and self.depth_array[self.depthX][self.depthY] < 0.6 :
if self.treeCatched == False:
self.state = 'tree'
elif self.appleCatched == False:
self.state = 'pickapple'
# State: home POSE: 0 -2.0e-01 +6.50e-01
# ORI: 0 0 0
def tree(self):
print("[ TREE ] ")
print("x: ", self.x, " color: ", self.color)
if self.x > 0 and self.color == 86: #tree detected
print("[+++] Tree detected")
depth = self.depth_array[self.depthX][self.depthY]
if depth > 0.46:
self.treeCatched = True
self.state = 'idle'
else:
for x in range(2):
self.reverse(False)
for x in range(2):
self.movex(False)
for x in range(2):
self.movey(False)
def pick_apple(self):
print("[ LOOKING FOR APPLE ]: ", self.depth_array[self.depthX][self.depthY])
print("x: ", self.x, " color: ", self.color)
self.state = 'idle'
if self.x > 0 and self.color == 10: #apple detected
print("[+++] Apple detected")
self.appleCatched = True
self.state = 'movex' #state = move x
#State: move coordinate x
def movex(self, state):
if self.x > 256:
self.moveDummy(y_=-0.00125)
if self.x < 256:
self.moveDummy(y_=0.00125)
if self.x >= 255 or self.x <= 256:
print("[MOVE X] = ", self.x)
if state == True:
self.state = 'movey' #state = move y
#State: move coordinate y
def movey(self, state):
if self.y > 225:
self.moveDummy(z_=0.00125)
if self.y < 200:
self.moveDummy(z_=-0.00125)
return
if self.y >= 224 or self.y <= 225:
print("[MOVE Y] = ", self.y)
if state == True:
self.state = 'advance' #state = advance
#State: advance until the suctionPad catches the apple
def advance(self, state):
print("[ADVANCE] depth = ", self.depth_array[self.depthX][self.depthY])
depth = self.depth_array[self.depthX][self.depthY]
if depth > 0.18:
self.moveDummy(x_=0.005)
else:
self.moveDummy(x_=0.00125)
if state == True:
# if depth > 0.165 and depth < 0.176 or self.y < 50:
# print("[---] DEPTH < ", depth)
# self.state = 'reverse' #state = reverse
# else:
# self.state = 'movex' #state = move x
self.closeDepth(self.depthX, self.depthY, 0, 0)
self.closeDepth(self.depthX, self.depthY, 0, 150)
self.closeDepth(self.depthX, self.depthY, 150, 0)
self.closeDepth(self.depthX, self.depthY, 150, 150)
def closeDepth(self,x,y,offset_x, offset_y):
depth = self.depth_array[x+offset_x][y+offset_y]
if depth > 0.165 and depth < 0.177 or self.y < 50:
print("[---] DEPTH < ", depth)
self.state = 'reverse' #state = reverse
else:
self.state = 'movex' #state = move x
# State: drop
def drop(self):
print("[ DROP ] depth: ", self.depth_array[self.depthX][self.depthY])
leave = RoboCompCoppeliaUtils.PoseType(x=1.00000000,y=1.00000000,z=1.00000000)
#catch = RoboCompCoppeliaUtils.PoseType(x=0.00000000,y=0.00000000,z=0.00000000)
for x in range(1000):
print("[ DETACHED ]")
self.coppeliautils_proxy.addOrModifyDummy(RoboCompCoppeliaUtils.TargetTypes.Hand, "suctionPad_Dummy", leave)
# for x in range(100):
# print("[ CATCH ]")
# self.coppeliautils_proxy.addOrModifyDummy(RoboCompCoppeliaUtils.TargetTypes.Hand, "suctionPad_Dummy", catch)
self.n_tags = 0
self.treeCatched = False
self.state = 'stop'
# State: stop
def stop(self):
for x in range(20):
print("[ STOP ]")
self.state = 'turnright'
# State: REVERSE
def reverse(self, state):
print("[ REVERSE ] depth: ", self.depth_array[self.depthX][self.depthY])
self.moveDummy(x_=-0.0025)
if state == True:
if self.depth_array[self.depthX][self.depthY] >= 0.35:
self.state = 'turnleft'
self.appleCatched = False #apple detection is disabled
# State: turnleft
def turn_left(self, state):
print("[ TURN LEFT ] ", self.depth_array[self.depthX][self.depthY])
if(self.n_tags == 0):
self.moveDummy(rz_=-0.007)
else:
print(" + TAG SPOTTED")
if state == True:
self.state = 'tag'
# State: turnright
def turn_right(self, state):
print("[ TURN RIGHT ] depth", self.depth_array[self.depthX][self.depthY])
if(self.depth_array[self.depthX][self.depthY] > 0.5):
self.moveDummy(rz_=0.005)
else:
if state == True:
self.state = 'idle'
print("RECTIFIED TURN RIGHT")
for x in range(20):
self.movex(False)
self.movey(False)
self.moveDummy(rz_=0.0025)
# State: tag
def tag(self):
print("[ TAG ] ")
print("Centro X del tag: ",self.center[0])
print("Centro Y del tag: ",self.center[1])
self.x = self.center[0]
self.y = self.center[1]
print("TAG rectified")
for x in range(2):
self.movex(False)
self.movey(False)
self.n_tags = 0
self.state = 'drop'
def error(self):
print("Error en el switch")
# =============== Object detection ===================
# ====================================================
def colorDetect(self, color_, low_h, low_s, low_v, high_h, high_s, high_v):
color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth)
x, y, x_, y_ = [0 for _ in range(4)]
first = False
color = cv.cvtColor(color, cv.COLOR_RGB2BGR)
hsv = cv.cvtColor(color, cv.COLOR_BGR2HSV)
lower_red_range = np.array([low_h, low_s, low_v])
upper_red_range = np.array([high_h, high_s, high_v])
mask = cv.inRange(hsv, lower_red_range, upper_red_range)
# Find contours
cnts = cv.findContours(mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# Iterate through contours and filter by the number of vertices
for c in cnts:
area = cv.contourArea(c)
if area > 700:
cv.drawContours(color,[c],-1,(0,255,0), 2)
M = cv.moments(c)
x = int(M["m10"]/ M["m00"])
y = int(M["m01"]/ M["m00"])
if(first == False):
x_ = x
y_ = y
first = True
cv.circle(color,(x,y),2,(0,255,0), 2)
# cv.putText(color, "rojo", (x-20, y-20), cv.FONT_HERSHEY_SIMPLEX,2, (255,255,255), 2)
color = cv.cvtColor(color, cv.COLOR_BGR2RGB)
self.draw_camera(color, 'Drone Camera')
return (x_, y_, high_h)
def ar_detection(self, color_):
color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth)
gray = cv.cvtColor(color, cv.COLOR_RGB2GRAY)
k = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cameraMatrix = np.array(k).reshape((3,3))
camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] )
tag_size = 0.065
self.tags = self.at_detector.detect(gray, False, camera_params, tag_size)
if(len(self.tags) == 1):
print("[ INFO ] {} total AprilTags detected".format(len(self.tags)))
self.n_tags = 1
if(self.n_tags == 1):
for tag in self.tags:
for idx in range(len(tag.corners)):
cv.line(color, tuple(tag.corners[idx-1, :].astype(int)),
tuple(tag.corners[idx, :].astype(int)), (0, 255, 0),thickness=2)
cv.circle(color, tuple(tag.center.astype(int)), 2, (0,255,0), 2)
self.center = tag.center.astype(int)
# print("Centro X del tag: ",self.center[0])
# print("Centro Y del tag: ",self.center[1])
self.draw_camera(color, 'Drone Camera')
def startup_check(self):
QTimer.singleShot(200, QApplication.instance().quit)
# =============== Slots methods for State Machine ===================
# ===================================================================
#
# sm_initialize
#
@QtCore.Slot()
def sm_initialize(self):
#print("Entered state initialize")
self.t_initialize_to_compute.emit()
pass
#
# sm_compute
#
@QtCore.Slot()
def sm_compute(self):
#print("Entered state compute")
self.compute()
pass
#
# sm_finalize
#
@QtCore.Slot()
def sm_finalize(self):
print("Entered state finalize")
pass
with open(test_images_path + '/test_info.yaml', 'r') as stream:
parameters = yaml.load(stream)
#### test WITH THE SAMPLE IMAGE ####
print("\n\nTESTING WITH A SAMPLE IMAGE")
img = cv2.imread(test_images_path+'/'+parameters['sample_test']['file'], cv2.IMREAD_GRAYSCALE)
cameraMatrix = numpy.array(parameters['sample_test']['K']).reshape((3,3))
camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] )
if visualization:
cv2.imshow('Original image',img)
tags = at_detector.detect(img, True, camera_params, parameters['sample_test']['tag_size'])
print(tags)
color_img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(color_img, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
cv2.putText(color_img, str(tag.tag_id),
org=(tag.corners[0, 0].astype(int)+10,tag.corners[0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
if visualization:
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
self.bridge = CvBridge()
#
# Write your code here
#
self.homography = self.load_extrinsics()
self.H = np.array(self.homography).reshape((3, 3))
self.Hinv = np.linalg.inv(self.H)
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.sub_compressed_img = rospy.Subscriber(
"/robot_name/camera_node/image/compressed",
CompressedImage,
self.callback,
queue_size=1
)
self.sub_camera_info = rospy.Subscriber(
"/robot_name/camera_node/camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1
)
self.pub_map_img = rospy.Publisher(
"~april_tag_duckie/image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION,
)
def callback(self, msg):
header = msg.header
cv_img = self.readImage(msg)
cameraMatrix = np.array(self.camera_model.K)
camera_params = (cameraMatrix[0, 0], cameraMatrix[1, 1], cameraMatrix[0, 2], cameraMatrix[1, 2])
gray_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(gray_img, True, camera_params, 0.065)
Ho = np.array(tags[0].homography)
print("first", Ho)
Ho_new = self.projection_matrix(cameraMatrix, Ho)
print("second", Ho_new)
img_out = self.renderer.render(cv_img, Ho_new)
msg_out = self.bridge.cv2_to_compressed_imgmsg(img_out, dst_format='jpeg')
msg_out.header = header
self.pub_map_img.publish(msg_out)
def cb_camera_info(self, msg):
# unsubscribe from camera_info
self.loginfo('Camera info message received. Unsubscribing from camera_info topic.')
# noinspection PyBroadException
try:
self.sub_camera_info.shutdown()
except BaseException:
self.loginfo('BaseException')
pass
# ---
self.H_camera, self.W_camera = msg.height, msg.width
# create new camera info
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(msg)
def projection_matrix(self, intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
#
# Write your code here
#
kinv = np.linalg.inv(intrinsic)
rt = np.dot(kinv, homography)
r1 = rt[:, 0]
r2 = rt[:, 1]
t = rt[:, 2]
r1_norm = r1
print(rt)
r2_new = r2 - np.dot(r1_norm, r2) * r1_norm
r2_norm = r2_new / np.linalg.norm(r2_new) * np.linalg.norm(r1)
r3_new = np.cross(r1_norm, r2_norm)
r3_norm = r3_new / np.linalg.norm(r3_new) * np.linalg.norm(r1)
matrix = np.zeros((3, 4))
matrix[:, 0] = r1_norm
matrix[:, 1] = r2_norm
matrix[:, 2] = r3_norm
matrix[:, 3] = t
h**o = np.dot(intrinsic, matrix)
return h**o
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self,fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
def load_extrinsics(self):
"""
Loads the homography matrix from the extrinsic calibration file.
Returns:
:obj:`numpy array`: the loaded homography matrix
"""
# load intrinsic calibration
cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
cali_file = cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(cali_file):
self.log("Can't find calibration file: %s.\n Using default calibration instead."
% cali_file, 'warn')
cali_file = (cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(cali_file):
msg = 'Found no calibration file ... aborting'
self.log(msg, 'err')
rospy.signal_shutdown(msg)
try:
with open(cali_file, 'r') as stream:
calib_data = yaml.load(stream)
except yaml.YAMLError:
msg = 'Error in parsing calibration file %s ... aborting' % cali_file
self.log(msg, 'err')
rospy.signal_shutdown(msg)
return calib_data['homography']
class ATPoseNode(DTROS):
"""
Computes an estimate of the Duckiebot pose using the wheel encoders.
Args:
node_name (:obj:`str`): a unique, descriptive name for the ROS node
Configuration:
Publisher:
~/rectified_image (:obj:`Image`): The rectified image
~at_localization (:obj:`PoseStamped`): The computed position broadcasted in TFs
Subscribers:
~/camera_node/image/compressed (:obj:`CompressedImage`): Observation from robot
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATPoseNode, self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# get the name of the robot
self.veh = rospy.get_namespace().strip("/")
self.bridge = CvBridge()
self.camera_info = None
self.Rectify = None
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Construct publishers
self.at_estimated_pose = rospy.Publisher(
f'/{self.veh}/at_localization',
Odometry,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Camera subscribers:
camera_topic = f'/{self.veh}/camera_node/image/compressed'
self.camera_feed_sub = rospy.Subscriber(camera_topic,
CompressedImage,
self.detectATPose,
queue_size=1,
buff_size=2**24)
camera_info_topic = f'/{self.veh}/camera_node/camera_info'
self.camera_info_sub = rospy.Subscriber(camera_info_topic,
CameraInfo,
self.getCameraInfo,
queue_size=1)
self.image_pub = rospy.Publisher(f'/{self.veh}/rectified_image',
Image,
queue_size=10)
self.tag_pub = rospy.Publisher(f'/{self.veh}/detected_tags',
Int32MultiArray,
queue_size=5)
self.log("Initialized!")
def getCameraInfo(self, cam_msg):
if (self.camera_info == None):
self.camera_info = cam_msg
self.Rectify = Rectify(self.camera_info)
return
def _broadcast_tf(self,
parent_frame_id: str,
child_frame_id: str,
stamp: Optional[float] = None,
translations: Optional[Tuple[float, float,
float]] = None,
euler_angles: Optional[Tuple[float, float,
float]] = None,
quarternion: Optional[List[float]] = None):
br = tf2_ros.StaticTransformBroadcaster()
ts = TransformStamped()
ts.header.frame_id = parent_frame_id
ts.child_frame_id = child_frame_id
if stamp is None:
stamp = rospy.Time.now()
ts.header.stamp = stamp
if translations is None:
translations = 0, 0, 0
ts.transform.translation.x = translations[0]
ts.transform.translation.y = translations[1]
ts.transform.translation.z = translations[2]
if euler_angles is not None:
q = tf_conversions.transformations.quaternion_from_euler(
*euler_angles)
elif quarternion is not None:
q = quarternion
else:
q = 0, 0, 0, 1
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
br.sendTransform(ts)
def _broadcast_detected_tag(self, image_msg, tag_id, tag):
# inverse the rotation and tranformation comming out of the AT detector.
# The AP detector's output is the camera frame relative to the TAG
# According to: https://duckietown.slack.com/archives/C6ZHPV212/p1619876209086300?thread_ts=1619751267.084600&cid=C6ZHPV212
# While the transformation below needs TAG relative to camera
# Therefore we need to reverse it first.
pose_R = np.identity(4)
pose_R[:3, :3] = tag.pose_R
inv_pose_R = np.transpose(pose_R)
pose_t = np.ones((4, 1))
pose_t[:3, :] = tag.pose_t
inv_pose_t = -np.matmul(inv_pose_R, pose_t)
out_q = quaternion_from_matrix(inv_pose_R)
out_t = inv_pose_t[:3, :]
tag_frame_id = 'april_tag_{}'.format(tag_id)
tag_cam_frame_id = 'april_tag_cam_{}'.format(tag_id)
camera_rgb_link_frame_id = 'at_{}_camera_rgb_link'.format(tag_id)
camera_link_frame_id = 'at_{}_camera_link'.format(tag_id)
base_link_frame_id = 'at_{}_base_link'.format(tag_id)
# ATag to ATag cam (this is because AprilTAG pose is different from physical)
self._broadcast_tf(parent_frame_id=tag_frame_id,
child_frame_id=tag_cam_frame_id,
euler_angles=(-90 * math.pi / 180, 0,
-90 * math.pi / 180))
# ATag cam to camera_rgb_link (again this is internal cam frame)
self._broadcast_tf(parent_frame_id=tag_cam_frame_id,
child_frame_id=camera_rgb_link_frame_id,
stamp=image_msg.header.stamp,
translations=out_t,
quarternion=out_q)
# camera_rgb_link to camera_link (internal cam frame to physical cam frame)
self._broadcast_tf(parent_frame_id=camera_rgb_link_frame_id,
child_frame_id=camera_link_frame_id,
stamp=image_msg.header.stamp,
euler_angles=(0, -90 * math.pi / 180,
90 * math.pi / 180))
# camera_link to base_link of robot
self._broadcast_tf(parent_frame_id=camera_link_frame_id,
child_frame_id=base_link_frame_id,
stamp=image_msg.header.stamp,
translations=(-0.066, 0, -0.106),
euler_angles=(0, -15 * math.pi / 180, 0))
def detectATPose(self, image_msg):
"""
Image callback.
Args:
msg_encoder (:obj:`Compressed`) encoder ROS message.
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
new_cam, rectified_img = self.Rectify.rectify_full(cv_image)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rectified_img, "bgr8"))
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
gray_img = cv2.cvtColor(rectified_img, cv2.COLOR_RGB2GRAY)
camera_params = (new_cam[0, 0], new_cam[1, 1], new_cam[0,
2], new_cam[1,
2])
detected_tags = self.at_detector.detect(gray_img,
estimate_tag_pose=True,
camera_params=camera_params,
tag_size=0.065)
detected_tag_ids = list(map(lambda x: fetch_tag_id(x), detected_tags))
array_for_pub = Int32MultiArray(data=detected_tag_ids)
for tag_id, tag in zip(detected_tag_ids, detected_tags):
print('detected {}: ({}, {})'.format(
tag_id, image_msg.header.stamp.to_time(),
rospy.Time.now().to_time()))
self._broadcast_detected_tag(image_msg, tag_id, tag)
self.tag_pub.publish(array_for_pub)
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# Load calibration files
self._res_w = 640
self._res_h = 480
self.calibration_call()
# Initialize classes
self.at_detec = Detector()
self.helper = Helpers(self.current_camera_info)
self.bridge = CvBridge()
# Subscribe to the compressed camera image
self.cam_image = rospy.Subscriber(
f'/{self.veh}/camera_node/image/compressed',
CompressedImage,
self.callback,
queue_size=10)
# Publishers
self.aug_image = rospy.Publisher(
f'/{self.veh}/{node_name}/image/compressed',
CompressedImage,
queue_size=10)
def callback(self, image):
"""
Callback method that ties everything together
"""
# convert the image to cv2 format
image = self.bridge.compressed_imgmsg_to_cv2(image)
#undistort the image
image = self.helper.process_image(image)
# detect AprilTags
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
tags = self.at_detec.detect(gray,
estimate_tag_pose=False,
camera_params=None,
tag_size=None)
for tag in tags:
# estimate homography
homography = tag.homography
# calculate projection matrix
proj_mat = self.projection_matrix(self.current_camera_info.K,
homography)
# rendering magics
image = self.renderer.render(image, proj_mat)
#publish image to new topic
self.aug_image.publish(self.bridge.cv2_to_compressed_imgmsg(image))
def projection_matrix(self, intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
# format the K matrix
K = np.reshape(np.array(intrinsic), (3, 3))
# calculate R_t
R_t = np.matmul(np.linalg.inv(K), homography)
# normalization parameters R_t
n_r1 = np.linalg.norm(R_t[:, 0])
n_r2 = np.linalg.norm(R_t[:, 1])
# calculate R_3 and rebuild R_t
R = np.array([
R_t[:, 0], R_t[:, 1],
(n_r1 + n_r2) / 2 * np.cross(R_t[:, 0] / n_r1, R_t[:, 1] / n_r2)
])
R_t = np.vstack((R, R_t[:, 2]))
# calculate the projection matrix
proj_mat = np.matmul(K, np.transpose(R_t))
return proj_mat
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def calibration_call(self):
# copied from the camera driver (dt-duckiebot-interface)
# For intrinsic calibration
self.cali_file_folder = '/data/config/calibrations/camera_intrinsic/'
self.frame_id = rospy.get_namespace().strip(
'/') + '/camera_optical_frame'
self.cali_file = self.cali_file_folder + rospy.get_namespace().strip(
"/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(self.cali_file):
self.logwarn(
"Calibration not found: %s.\n Using default instead." %
self.cali_file)
self.cali_file = (self.cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(self.cali_file):
rospy.signal_shutdown("Found no calibration file ... aborting")
# Load the calibration file
self.original_camera_info = self.load_camera_info(self.cali_file)
self.original_camera_info.header.frame_id = self.frame_id
self.current_camera_info = copy.deepcopy(self.original_camera_info)
self.update_camera_params()
self.log("Using calibration file: %s" % self.cali_file)
# extrinsic calibration
self.ex_cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
self.ex_cali_file = self.ex_cali_file_folder + rospy.get_namespace(
).strip("/") + ".yaml"
self.ex_cali = self.readYamlFile(self.ex_cali_file)
# define homography
self.homography = self.ex_cali["homography"]
def update_camera_params(self):
""" Update the camera parameters based on the current resolution.
The camera matrix, rectification matrix, and projection matrix depend on
the resolution of the image.
As the calibration has been done at a specific resolution, these matrices need
to be adjusted if a different resolution is being used.
TODO: Test that this really works.
"""
scale_width = float(self._res_w) / self.original_camera_info.width
scale_height = float(self._res_h) / self.original_camera_info.height
scale_matrix = np.ones(9)
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[4] *= scale_height
scale_matrix[5] *= scale_height
# Adjust the camera matrix resolution
self.current_camera_info.height = self._res_h
self.current_camera_info.width = self._res_w
# Adjust the K matrix
self.current_camera_info.K = np.array(
self.original_camera_info.K) * scale_matrix
# Adjust the P matrix (done by Rohit)
scale_matrix = np.ones(12)
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[5] *= scale_height
scale_matrix[6] *= scale_height
self.current_camera_info.P = np.array(
self.original_camera_info.P) * scale_matrix
@staticmethod
def load_camera_info(filename):
"""Loads the camera calibration files.
Loads the intrinsic camera calibration.
Args:
filename (:obj:`str`): filename of calibration files.
Returns:
:obj:`CameraInfo`: a CameraInfo message object
"""
with open(filename, 'r') as stream:
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def onShutdown(self):
super(ARNode, self).onShutdown()
class Vision:
coral_model_file = "/home/pi/catkin_ws/src/robot5/models/mobilenet_ssd_v2/mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite"
coral_labels_file = "/home/pi/catkin_ws/src/robot5/models/mobilenet_ssd_v2/coco_labels.txt"
coral_confidence = 0.3
caffe_model_file = "/home/pi/catkin_ws/src/robot5/models/res10_300x300_ssd_iter_140000.caffemodel"
caffe_confidence = 0.5
caffe_prototxt = "/home/pi/catkin_ws/src/robot5/models/deploy.prototxt.txt"
face_cascade = cv2.CascadeClassifier('/home/pi/opencv/data/haarcascades/haarcascade_frontalface_default.xml')
eye_cascade = cv2.CascadeClassifier('/home/pi/opencv/data/haarcascades/haarcascade_eye.xml')
APRILWIDTH = 172
FOCALLENGTH = 0.304
def __init__(self):
self.coral_model = {}
self.coral_labels = {}
self.caffe_model = {}
self.at_detector = {}
self.videoStream = {}
self.status = None
self.captureFrame = None
self.visionFrame = None
self.thread = Thread(target=self.frameUpdate,args=())
self.thread.daemon = True
self.flaskThread = Thread(target=self.runFlask)
self.flaskThread.daemon = True
self.frameLock = Lock()
print("[INFO] Initialising ROS...")
#self.pub = rospy.Publisher(name='vision_detect',subscriber_listener=vision_detect,queue_size=5,data_class=vision_detect)
self.pub = rospy.Publisher('/vision_detect',vision_detect)
rospy.init_node('robot5_vision',anonymous=False)
for row in open(self.coral_labels_file):
(classID, label) = row.strip().split(maxsplit=1)
self.coral_labels[int(classID)] = label.strip()
print("[INFO] loading Coral model...")
self.coral_model = DetectionEngine(self.coral_model_file)
#print("[INFO] loading Caffe model...")
#self.caffe_model = cv2.dnn.readNetFromCaffe(self.caffe_prototxt, self.caffe_model_file)
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
print("[INFO] Running Flask...")
self.app = Flask(__name__)
self.add_routes()
self.flaskThread.start()
print("[INFO] starting video stream...")
self.videoStream = VideoStream(src=0,usePiCamera=True).start()
time.sleep(2.0) # warmup
self.captureFrame = self.videoStream.read()
self.visionFrame = self.captureFrame
self.thread.start()
time.sleep(0.5) # get first few
srun = True
print("[INFO] running frames...")
while srun:
srun = self.doFrame()
cv2.destroyAllWindows()
self.videoStream.stop()
def frameUpdate(self):
while True:
self.captureFrame = self.videoStream.read()
time.sleep(0.03)
def gen(self):
while True:
if self.visionFrame is not None:
bout = b"".join([b'--frame\r\nContent-Type: image/jpeg\r\n\r\n', self.visionFrame,b'\r\n'])
yield (bout)
else:
return ""
def add_routes(self):
@self.app.route("/word/<word>")
def some_route(word):
self.testout()
return "At some route:"+word
@self.app.route('/video_feed')
def video_feed():
return Response(self.gen(),mimetype='multipart/x-mixed-replace; boundary=frame')
def testout(self):
print("tested")
pass
def runFlask(self):
self.app.run(debug=False, use_reloader=False,host='0.0.0.0', port=8000)
def coralDetect(self,frame,orig):
start1 = time.time()
results = self.coral_model.detect_with_image(frame, threshold=self.coral_confidence,keep_aspect_ratio=True, relative_coord=False)
fcount = 0
points = []
for r in results:
box = r.bounding_box.flatten().astype("int")
(startX, startY, endX, endY) = box
label = self.coral_labels[r.label_id]
cv2.rectangle(orig, (startX, startY), (endX, endY), (0, 255, 0), 2)
y = startY - 15 if startY - 15 > 15 else startY + 15
cx = startX + (endX - startX/2)
cy = startY + (endY - startY/2)
#points.append({"type":"coral","num":fcount,"x":cx,"y":cy,"label":label,"score":r.score,"time":time.time() })
points.append(["coral",fcount,int(cx),int(cy),label,int(r.score*100),rospy.Time.now()])
fcount +=1
text = "{}: {:.2f}%".format(label, r.score * 100)
cv2.putText(orig, text, (startX, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
end1 = time.time()
#print("#1:",end1-start1)
return orig,points
def caffeDetect(self,frame,orig):
start2 = time.time()
(h, w) = frame.shape[:2]
blob = cv2.dnn.blobFromImage(cv2.resize(frame, (300, 300)), 1.0,(300, 300), (104.0, 177.0, 123.0))
self.caffe_model.setInput(blob)
detections = self.caffe_model.forward()
fcount = 0
points = []
for i in range(0, detections.shape[2]):
confidence = detections[0, 0, i, 2]
if confidence < self.caffe_confidence:
continue
box = detections[0, 0, i, 3:7] * np.array([w, h, w, h])
(startX, startY, endX, endY) = box.astype("int")
text = "{:.2f}%".format(confidence * 100)
y = startY - 10 if startY - 10 > 10 else startY + 10
cx = startX + (endX - startX/2)
cy = startY + (endY - startY/2)
#points.append({"type":"caffe","num":fcount,"x":cx,"y":cy,"score":confidence,"time":time.time()})
points.append(["caffe",fcount,int(cx),int(cy),"",int(confidence*10),rospy.Time.now()])
fcount +=1
cv2.rectangle(orig, (startX, startY), (endX, endY),(0, 0, 255), 2)
cv2.putText(orig, text, (startX, y), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2)
end2 = time.time()
#print("#2:",end2-start2)
return orig,points
def aprilDetect(self,grey,orig):
start3 = time.time()
Xarray = [338.563277422543, 0.0, 336.45495347495824, 0.0, 338.939280638548, 230.486982216255, 0.0, 0.0, 1.0]
camMatrix = np.array(Xarray).reshape((3,3))
params = (camMatrix[0,0],camMatrix[1,1],camMatrix[0,2],camMatrix[1,2])
tags = self.at_detector.detect(grey,True,params,0.065)
fcount = 0
points =[]
for tag in tags:
pwb = tag.corners[2][1] - tag.corners[0][1]
pwt = tag.corners[3][1] - tag.corners[1][1]
pwy = (pwb+pwt)/2
pwl = tag.corners[3][1] - tag.corners[0][1]
pwr = tag.corners[2][1] - tag.corners[1][1]
pwx = (pwl+pwr)/2
dist = self.distanceToCamera(self.APRILWIDTH,(pwx))
#print(dist)
#points.append({"type":"april","num":fcount,"x":pwx,"y":pwy,"label":tag.id,"score":dist,"time":time.time()})
points.append(["april",fcount,int(pwl + (pwx/2)),int(pwb + (pwy/2)),str(tag.tag_id)+"|"+str(dist),0,rospy.Time.now()])
fcount += 1
cv2.putText(orig, str(dist), (int(pwx), int(pwy)), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2)
for idx in range(len(tag.corners)):
cv2.line(orig,tuple(tag.corners[idx-1,:].astype(int)),tuple(tag.corners[idx,:].astype(int)),(0,255,0))
end3 = time.time()
#print("#3:",end3-start3)
return orig,points
def haarDetect(self,grey,orig):
start4 = time.time()
faces = self.face_cascade.detectMultiScale(grey,1.3,5)
fcount=0
points =[]
for(x,y,w,h) in faces:
#points.append({"type":"haar","num":fcount,"x":x+(w/2),"y":y+(h/2),"time":time.time()})
points.append(["haar",fcount,int(x+(w/2)),int(y+(h/2)),"",0,rospy.Time.now()])
orig = cv2.rectangle(orig,(x,y),(x+w,y+h),(255,255,0),2)
roi_gray = grey[y:y+h, x:x+w]
roi_color = orig[y:y+h, x:x+w]
fcount += 1
eyes = self.eye_cascade.detectMultiScale(roi_gray)
for (ex,ey,ew,eh) in eyes:
cv2.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,255,0),2)
end4 = time.time()
#print("#4:",end4-start4)
return orig,points
def doFrame(self):
frame = self.captureFrame
if frame is None:
return False
frame = imutils.resize(frame, width=500)
orig = frame.copy()
grey = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = Image.fromarray(frame)
outframe,cpoints = self.coralDetect(frame,orig)
outframe,apoints = self.aprilDetect(grey,outframe)
outframe,hpoints = self.haarDetect(grey,outframe)
#outframe,fpoints = self.caffeDetect(orig,outframe)
points = list(itertools.chain(cpoints,apoints,hpoints))
for p in points:
self.pub.publish(p[0],p[1],p[2],p[3],p[4],p[5],p[6])
pass
ret, self.visionFrame = cv2.imencode('.jpg', outframe)
#self.visionFrame = outframe
#cv2.imshow("Frame", outframe)
#key = cv2.waitKey(1) & 0xFF
#if key == ord("q"):
# return False
return True
def distanceToCamera(self,actWidth,perWidth):
return ((actWidth*self.FOCALLENGTH)/perWidth)*2
class ATLocalizationNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATLocalizationNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
# State variable for the robot
self.pose = Pose2D(0.27, 0.0, np.pi) # Initial state given arbitrarily
# Static transforms
# Map -> Baselink. To be computed
self.T_map_baselink = TransformStamped()
self.T_map_baselink.header.frame_id = 'map'
self.T_map_baselink.child_frame_id = 'at_baselink'
# Map -> Apriltag. STATIC
self._T_map_apriltag = TransformStamped()
self._T_map_apriltag.header.frame_id = 'map'
self._T_map_apriltag.header.stamp = rospy.Time.now()
self._T_map_apriltag.child_frame_id = 'apriltag'
self._T_map_apriltag.transform.translation.x = 0.0
self._T_map_apriltag.transform.translation.y = 0.0
self._T_map_apriltag.transform.translation.z = 0.09 # Height of 9 cm
q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
self._T_map_apriltag.transform.rotation.x = q[0]
self._T_map_apriltag.transform.rotation.y = q[1]
self._T_map_apriltag.transform.rotation.z = q[2]
self._T_map_apriltag.transform.rotation.w = q[3]
# Apriltag -> Camera. Computed using apriltag detection
self.T_apriltag_camera = TransformStamped()
self.T_apriltag_camera.header.frame_id = 'apriltag'
self.T_apriltag_camera.child_frame_id = 'camera'
# Camera -> Baselink. STATIC
self._T_camera_baselink = TransformStamped()
self._T_camera_baselink.header.frame_id = 'camera'
self._T_camera_baselink.header.stamp = rospy.Time.now()
self._T_camera_baselink.child_frame_id = 'at_baselink'
self._T_camera_baselink.transform.translation.x = -0.0582
self._T_camera_baselink.transform.translation.y = 0.0
self._T_camera_baselink.transform.translation.z = -0.110
q = tf_conversions.transformations.quaternion_from_euler(
0.0, np.deg2rad(-15), 0.0)
self._T_camera_baselink.transform.rotation.x = q[0]
self._T_camera_baselink.transform.rotation.y = q[1]
self._T_camera_baselink.transform.rotation.z = q[2]
self._T_camera_baselink.transform.rotation.w = q[3]
# Transformation Matrices to ease computation
R_MA = tf_conversions.transformations.euler_matrix(
0.0, 0.0, 0.0, 'rxyz')
t_MA = tf_conversions.transformations.translation_matrix(
np.array([0.0, 0.0, 0.09]))
self.T_MA = tf_conversions.transformations.concatenate_matrices(
t_MA, R_MA)
R_CB = tf_conversions.transformations.euler_matrix(
0.0, np.deg2rad(-15.0), 0.0, 'rxyz')
t_CB = tf_conversions.transformations.translation_matrix(
np.array([-0.0582, 0.0, -0.1072]))
self.T_CB = tf_conversions.transformations.concatenate_matrices(
t_CB, R_CB)
# Define rotation matrices to follow axis convention in rviz when using apriltag detection
self.T_ApA = tf_conversions.transformations.euler_matrix(
np.pi / 2.0, 0.0, -np.pi / 2.0, 'rxyz')
self.T_CCp = tf_conversions.transformations.euler_matrix(
-np.pi / 2.0, 0.0, -np.pi / 2.0, 'rzyx')
# Load calibration files
self.calib_data = self.readYamlFile(
'/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml')
self.log('Loaded intrinsics calibration file')
self.extrinsics = self.readYamlFile(
'/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml')
self.log('Loaded extrinsics calibration file')
# Retrieve intrinsic info
cam_info = self.setCamInfo(self.calib_data)
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(cam_info)
# Initiate maps for rectification
self._init_rectify_maps()
# Create AprilTag detector object
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Create cv_bridge
self.bridge = CvBridge()
# Define subscriber to recieve images
self.image_sub = rospy.Subscriber(
'/' + self.veh + '/camera_node/image/compressed', CompressedImage,
self.callback)
# Publishers and broadcasters
self.pub_robot_pose_tf = rospy.Publisher('~at_baselink_transform',
TransformStamped,
queue_size=1)
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
self.tfBroadcaster = tf.TransformBroadcaster(queue_size=1)
self.log(node_name + ' initialized and running')
def callback(self, ros_image):
'''
MAIN TASK
Convert the image from the camera_node to a cv2 image. Then, detects
the apriltags position and retrieves the Rotation and translation to it from the
camera frame, and next computes T_map_baselink by concatenating transformations.
The resultant transformation is published, as well as all intermedium are broadcasted
'''
# Convert to cv2 image
image = self.readImage(ros_image)
# Rectify image
image = self.processImage(image)
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Extract camera parameters
K = np.array(self.cam_model.K).reshape((3, 3))
cam_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
# Detect apriltags
detections = self.at_detector.detect(gray_image,
estimate_tag_pose=True,
camera_params=cam_params,
tag_size=0.065)
# If an apriltag is detected, update transformations
for tag in detections:
# Retrieve rotation and translation from apriltag to camera and homogeneize
R_CA = tf_conversions.transformations.identity_matrix()
R_CA[:3, :3] = tag.pose_R
p = tag.pose_t.T[0]
t_CA = tf_conversions.transformations.translation_matrix(
np.array([p[0], p[1], p[2], 1.0]))
# Define homogeneous transformation matrix
T_CpAp = tf_conversions.transformations.concatenate_matrices(
t_CA, R_CA)
# Compute apriltag->camera transform
self.T_CA = tf_conversions.transformations.concatenate_matrices(
self.T_CCp, T_CpAp, self.T_ApA)
self.T_AC = tf_conversions.transformations.inverse_matrix(
self.T_CA)
# Compute map->baselink transform
self.T_MB = tf_conversions.transformations.concatenate_matrices(
self.T_MA, self.T_AC, self.T_CB)
# Obtain TransformStamped messages
self.T_apriltag_camera.header.stamp = ros_image.header.stamp
t = tf_conversions.transformations.translation_from_matrix(
self.T_AC)
self.T_apriltag_camera.transform.translation.x = t[0]
self.T_apriltag_camera.transform.translation.y = t[1]
self.T_apriltag_camera.transform.translation.z = t[2]
q = tf_conversions.transformations.quaternion_from_matrix(
self.T_AC)
self.T_apriltag_camera.transform.rotation.x = q[0]
self.T_apriltag_camera.transform.rotation.y = q[1]
self.T_apriltag_camera.transform.rotation.z = q[2]
self.T_apriltag_camera.transform.rotation.w = q[3]
self.T_map_baselink.header.stamp = ros_image.header.stamp
t = tf_conversions.transformations.translation_from_matrix(
self.T_MB)
self.T_map_baselink.transform.translation.x = t[0]
self.T_map_baselink.transform.translation.y = t[1]
self.T_map_baselink.transform.translation.z = t[2]
q = tf_conversions.transformations.quaternion_from_matrix(
self.T_MB)
self.T_map_baselink.transform.rotation.x = q[0]
self.T_map_baselink.transform.rotation.y = q[1]
self.T_map_baselink.transform.rotation.z = q[2]
self.T_map_baselink.transform.rotation.w = q[3]
# Publish transform map -> baselink
self.pub_robot_pose_tf.publish(self.T_map_baselink)
# Broadcast all transforms
self.static_tf_br.sendTransform(self._T_map_apriltag)
self.static_tf_br.sendTransform(self._T_camera_baselink)
self.tfBroadcaster.sendTransformMessage(self.T_apriltag_camera)
def setCamInfo(self, calib_data):
'''
Introduces the camera information from the dictionary
obtained reading the yaml file to a CameraInfo object
'''
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def _init_rectify_maps(self):
# Get new optimal camera matrix
W = self.cam_model.width
H = self.cam_model.height
rect_camera_K, _ = cv2.getOptimalNewCameraMatrix(
self.cam_model.K, self.cam_model.D, (W, H), 1.0)
# Initialize rectification maps
mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.cam_model.K,
self.cam_model.D, np.eye(3),
rect_camera_K, (W, H),
cv2.CV_32FC1, mapx, mapy)
self.cam_model.K = rect_camera_K
self.mapx = mapx
self.mapy = mapy
def processImage(self, raw_image, interpolation=cv2.INTER_LINEAR):
'''
Undistort a provided image using the calibrated camera info
Implementation based on: https://github.com/duckietown/dt-core/blob/952ebf205623a2a8317fcb9b922717bd4ea43c98/packages/image_processing/include/image_processing/rectification.py
Args:
raw_image: A CV image to be rectified
interpolation: Type of interpolation. For more accuracy, use another cv2 provided constant
Return:
Undistorted image
'''
image_rectified = np.empty_like(raw_image)
processed_image = cv2.remap(raw_image, self.mapx, self.mapy,
interpolation, image_rectified)
return processed_image
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ATLocalizationNode, self).onShutdown()
