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 __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 __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 __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 __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 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')
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)
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 __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 __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 __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 __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 __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__(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 __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)
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
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 __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)
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 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_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 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()
try:
from cv2 import imshow
except:
print("The function imshow was not implemented in this installation. Rebuild OpenCV from source to use it")
print("VIsualization will be disabled.")
visualization = False
try:
import yaml
except:
raise Exception('You need yaml in order to run the tests. However, you can still use the library without it.')
at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
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] )
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)))
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()
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)
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
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.ndetectors = rospy.get_param('~ndetectors', 1)
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)
self.rectify_alpha = rospy.get_param('~rectify_alpha', 0.0)
# 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._mapx, self._mapy = None, None
# create detector object
self._detectors = [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
) for _ in range(self.ndetectors)]
self._renderer_busy = False
# create a CV bridge object
self._jpeg = TurboJPEG()
# create subscribers
self._img_sub = rospy.Subscriber(
'~image',
CompressedImage,
self._img_cb,
queue_size=1,
buff_size='20MB'
)
self._cinfo_sub = rospy.Subscriber(
'~camera_info',
CameraInfo,
self._cinfo_cb,
queue_size=1
)
# create publisher
self._tag_pub = rospy.Publisher(
'~detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_help='Tag detections',
)
self._img_pub = rospy.Publisher(
'~detections/image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION,
dt_help='Camera image with tag publishs superimposed',
)
# create thread pool
self._workers = ThreadPoolExecutor(self.ndetectors)
self._tasks = [None] * self.ndetectors
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()
# import the opencv library
import cv2
from dt_apriltags import Detector
import numpy as np
import time
import operator
at_detector = Detector(families='tag16h5',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
camera_params = (336.7755634193813, 336.02729840829176, 333.3575643300718,
212.77376312080065)
capture_duration = 8
# define a video capture object
vid = cv2.VideoCapture(0)
# Check if camera opened successfully
if (vid.isOpened() == False):
print("Error opening video file")
def checkForTags():
start_time = time.time()
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 []
