def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
TODO docs
"""
self.compass = VisualCompass(config)
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_ground_truth_images_count'):
self.ground_truth_images_count = 0
self.processed_set_all_ground_truth_images = False
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | ground truth images: %(ground_truth_count)d'
% {
'type':
config['compass_type'],
'matcher':
config['compass_matcher'],
'ground_truth_count':
config['compass_multiple_ground_truth_images_count']
})
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
# Register message server to call set truth callback
if self.changed_config_param(config, 'ground_truth_trigger_topic') or \
self.changed_config_param(config, 'ground_truth_trigger_queue_size'):
if hasattr(self, 'sub_trigger_set_ground_truth'):
self.sub_image_msg.unregister()
self.sub_trigger_set_ground_truth = rospy.Subscriber(
config['ground_truth_trigger_topic'],
Header,
self.set_truth_callback,
queue_size=config['ground_truth_trigger_queue_size'])
self.config = config
self.check_ground_truth_images_count()
return self.config
def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
Dynamic reconfigure callback that sets a new configuration
"""
self.compass = VisualCompass(config)
feature_map, meta_data = self.load_feature_map(
config['feature_map_file_path'])
self.compass.set_feature_map(feature_map)
self.compass.set_mean_feature_count(meta_data['mean_feature_count'])
self.filter = VisualCompassFilter()
if self.changed_config_param(config, 'feature_map_file_path'):
self.is_feature_map_set = False
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_map_image_count'):
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | map images: %(feature_map_count)d'
% {
'type': config['compass_type'],
'matcher': config['compass_matcher'],
'feature_map_count':
config['compass_multiple_map_image_count']
})
# Subscribe to game state
self.game_state_msg = rospy.Subscriber('gamestate',
GameState,
self.gamestate_callback,
queue_size=1,
tcp_nodelay=True)
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
self.config = config
return self.config
def __init__(self):
dirname = os.path.dirname(__file__)
relative_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
source = config['dummy_handler_input']
if isinstance(source, string):
root_folder = os.curdir
source = root_folder + source
self.video_getter = Videocv(source)
self.video_getter.run()
self.vc = VisualCompass(config)
self.loop(config)
def __init__(self, dimensions, angle_steps):
self.dimensions = dimensions
self.angle_steps = angle_steps
dirname = os.path.dirname(__file__)
relative_config_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_config_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
relative_data_path = config['evaluation_data']
self.data_path = os.path.join(dirname, relative_data_path)
self.loader = DataLoader(self.data_path, self.dimensions, self.angle_steps)
config['compass_type'] = 'multiple'
self.sample_count = 2 if config['compass_type'] == 'binary' else config['compass_multiple_ground_truth_images_count']
self.vc = VisualCompass(config)
def initialize_evaluation(config_table_entry):
dimensions = (10, 7)
angle_steps = 16
dirname = os.path.dirname(__file__)
relative_config_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_config_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
relative_data_path = config['evaluation_data']
data_path = os.path.join(dirname, relative_data_path)
loader = DataLoader(data_path, dimensions, angle_steps)
config["compass_type"] = config_table_entry["compass"]
config["compass_matcher"] = config_table_entry["matcher"]
config["compass_multiple_ground_truth_images_count"] = config_table_entry["samples"]
compass = VisualCompass(config)
print (config_table_entry["compass"] + " " + config_table_entry["matcher"] + " " + str(config_table_entry["samples"]))
return compass, loader, dimensions
class VisualCompassDummyHandler():
"""
Implements a ROS independent handler for a Visual Compass worker.
"""
def __init__(self):
dirname = os.path.dirname(__file__)
relative_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
source = config['dummy_handler_input']
if isinstance(source, string):
root_folder = os.curdir
source = root_folder + source
self.video_getter = Videocv(source)
self.video_getter.run()
self.vc = VisualCompass(config)
self.loop(config)
def debug_image_callback(self, debug_image):
"""
param debug_image: shows this debug image
"""
cv2.imshow("Video", debug_image)
def data_callback(self, angle, confidence):
"""
Get the data call back of the visual compass
param angle: angle calculated by the visual compass
param confidence: confidence of that angle
"""
print("Angle: {} | Confidence: {}".format(angle, confidence))
def loop(self, config):
"""
Processes stuff, feeds the compass with images, and sets the map
param config: config dict
"""
side = 0
while True:
image = self.video_getter.frame
k = cv2.waitKey(1)
#TODO remove
#self.debug_image_callback(image)
sides = config['compass_multiple_map_image_count'] if config[
'compass_type'] == 'multiple' else 2
if side < sides:
self.debug_image_callback(image)
# Wurde SPACE gedrueckt
if k % 256 == 32:
angle = float(side) / sides * math.pi * 2
self.vc.set_truth(angle, image)
side += 1
else:
self.vc.process_image(image,
resultCB=self.data_callback,
debugCB=self.debug_image_callback)
# Abbrechen mit ESC
if k % 256 == 27 or 0xFF == ord('q') or self.video_getter.ended:
break
self.video_getter.stop()
cv2.destroyAllWindows()
class MultipleEvaluator(object):
def __init__(self, dimensions, angle_steps):
self.dimensions = dimensions
self.angle_steps = angle_steps
dirname = os.path.dirname(__file__)
relative_config_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_config_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
relative_data_path = config['evaluation_data']
self.data_path = os.path.join(dirname, relative_data_path)
self.visualization_path = os.path.join(dirname,
config["visualization"])
self.loader = DataLoader(self.data_path, self.dimensions,
self.angle_steps)
self.sample_count = 2 if config['compass_type'] == 'binary' else config[
'compass_multiple_map_image_count']
self.vc = VisualCompass(config)
def show_img(self, image):
cv2.imshow("Record", image)
k = cv2.waitKey(1)
def evaluate(self):
self.set_truth()
self.evaluate_all_images()
def set_truth(self):
for i in range(self.sample_count):
angle = float(i) / self.sample_count * math.radians(360)
angle = (angle + math.radians(90)) % math.radians(360)
image = self.loader.get_image(4, 3, angle)
self.vc.set_truth(angle, image)
self.show_img(image)
cv2.destroyAllWindows()
def evaluate_all_images(self):
for i in range(16):
plt.figure(i)
self.evaluate_direction(math.pi / 8 * i, None)
print(i)
print("done")
filename = "" + str(i) + ".png"
plt.savefig(os.path.join(self.visualization_path, filename))
def debug_image_callback(self, debug_image):
return
self.show_img(debug_image)
time.sleep(0.5)
def evaluate_direction(self, step, ax):
ax = plt.subplot(1, 2, 1)
U = np.zeros(self.dimensions)
V = np.zeros(self.dimensions)
C = np.zeros(self.dimensions)
for i in range(self.dimensions[0]):
for j in range(self.dimensions[1]):
image = self.loader.get_image(i, j, step)
angle, confidence = self.vc.process_image(image)
z = np.exp(1j * angle)
U[i, j] = np.real(z) * confidence
V[i, j] = np.imag(z) * confidence
C[i, j] = confidence
normalizer = colors.Normalize(0, 1)
quiver = ax.quiver(U,
V,
C,
pivot='mid',
scale_units='xy',
scale=1,
norm=normalizer)
# Q = ax.quiver(U, V, C, pivot='mid')
ax.axis('equal')
ax.axis('off')
plt.subplot(1, 2, 2)
plt.imshow(self.loader.get_image(4, 3, step))
class VisualCompassSetup():
# type: () -> None
"""
TODO docs
Subscribes to raw image
Trigger: 'trigger_visual_compass'-trigger
Gets triggered e.i. while looking at a goal side
Returns side
"""
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_setup')
rospy.loginfo('Initializing visual compass setup')
self.bridge = CvBridge()
self.config = {}
self.image_msg = None
self.compass = None
self.hostname = socket.gethostname()
# TODO: docs
self.base_frame = 'base_footprint'
self.camera_frame = 'camera_optical_frame'
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(50))
self.listener = tf2.TransformListener(self.tf_buffer)
self.pub_head_mode = rospy.Publisher('head_mode',
HeadMode,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.logwarn("------------------------------------------------")
rospy.logwarn("|| WARNING ||")
rospy.logwarn("||Please remove the LAN cable from the Robot, ||")
rospy.logwarn("||after pressing 'YES' you have 10 Seconds ||")
rospy.logwarn("||until the head moves OVER the LAN port!!! ||")
rospy.logwarn("------------------------------------------------\n\n")
try:
input = raw_input
except NameError:
pass
accept = input("Do you REALLY want to start? (YES/n)")
if accept == "YES":
rospy.logwarn("REMOVE THE LAN CABLE NOW!!!!!")
rospy.sleep(10)
head_mode = HeadMode()
head_mode.headMode = 10
self.pub_head_mode.publish(head_mode)
rospy.loginfo("Head mode has been set!")
rospy.spin()
else:
rospy.signal_shutdown(
"You aborted the process! Shuting down correctly.")
def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
TODO docs
"""
self.compass = VisualCompass(config)
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_ground_truth_images_count'):
self.ground_truth_images_count = 0
self.processed_set_all_ground_truth_images = False
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | ground truth images: %(ground_truth_count)d'
% {
'type':
config['compass_type'],
'matcher':
config['compass_matcher'],
'ground_truth_count':
config['compass_multiple_ground_truth_images_count']
})
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
# Register message server to call set truth callback
if self.changed_config_param(config, 'ground_truth_trigger_topic') or \
self.changed_config_param(config, 'ground_truth_trigger_queue_size'):
if hasattr(self, 'sub_trigger_set_ground_truth'):
self.sub_image_msg.unregister()
self.sub_trigger_set_ground_truth = rospy.Subscriber(
config['ground_truth_trigger_topic'],
Header,
self.set_truth_callback,
queue_size=config['ground_truth_trigger_queue_size'])
self.config = config
self.check_ground_truth_images_count()
return self.config
def set_truth_callback(self, request):
if self.image_msg:
# TODO: check timestamps
orientation = self.tf_buffer.lookup_transform(
self.base_frame,
self.camera_frame,
self.image_msg.header.stamp,
timeout=rospy.Duration(0.5)).transform.rotation
yaw_angle = (euler_from_quaternion(
(orientation.x, orientation.y, orientation.z,
orientation.w))[2] + 0.5 * math.pi) % (2 * math.pi)
image = self.bridge.imgmsg_to_cv2(self.image_msg, 'bgr8')
self.compass.set_truth(yaw_angle, image)
self.ground_truth_images_count += 1
self.check_ground_truth_images_count()
else:
rospy.logwarn('No image received yet.')
def image_callback(self, image_msg):
# type: (Image) -> None
"""
TODO docs
"""
# Drops old images
# TODO: fix
# image_age = rospy.get_rostime() - image_msg.header.stamp
# if image_age.to_sec() > 0.1:
# print("Visual Compass: Dropped Image-message") # TODO debug printer
# return
self.image_msg = image_msg
def check_ground_truth_images_count(self):
# type: () -> None
"""
TODO docs
"""
config_ground_truth_images_count = self.config[
'compass_multiple_ground_truth_images_count']
if self.ground_truth_images_count != config_ground_truth_images_count:
rospy.loginfo(
'Visual compass: %(var)d of %(config)d ground truth images set. More images are needed.'
% {
'var': self.ground_truth_images_count,
'config': config_ground_truth_images_count
})
self.processed_set_all_ground_truth_images = False
else:
if not (self.processed_set_all_ground_truth_images):
rospy.loginfo(
'Visual compass: All ground truth images have been processed.'
)
self.save_ground_truth(self.config['ground_truth_file_path'])
self.processed_set_all_ground_truth_images = True
def save_ground_truth(self, ground_truth_file_path):
# type (str) -> None
"""
TODO docs
"""
converter = KeyPointConverter()
# get keypoints
features = self.compass.get_ground_truth_features()
# convert keypoints to basic values
keypoints = features[0]
keypoint_values = [converter.key_point2values(kp) for kp in keypoints]
descriptors = features[1]
meta = {
'field':
self.config['ground_truth_field'],
'date':
datetime.now(),
'device':
self.hostname,
'compass_type':
self.config['compass_type'],
'compass_matcher':
self.config['compass_matcher'],
'compass_multiple_ground_truth_images_count':
self.config['compass_multiple_ground_truth_images_count'],
'keypoint_count':
len(keypoint_values),
'descriptor_count':
len(descriptors)
}
dump_features = {
'keypoint_values': keypoint_values,
'descriptors': descriptors,
'meta': meta
}
# generate file path
file_path = self.package_path + ground_truth_file_path
# warn, if file does exist allready
if path.isfile(file_path):
rospy.logwarn(
'Ground truth file at: %(path)s does ALLREADY EXIST. This will be overwritten.'
% {'path': file_path})
# save keypoints in pickle file
with open(file_path, 'wb') as f:
pickle.dump(dump_features, f)
info_str = "\n\t-----------------------------------------------------------------------------------------------------------------\n" + \
"\tSaved ground truth file at: %(path)s\n" % {'path': file_path} + \
"\tRUN the following command on your system (NOT THE ROBOT) to save the ground truth file in your current directory:\n" + \
"\n\tscp bitbots@%(host)s:%(path)s .\n" % {'path': file_path, 'host': self.hostname} + \
"\t-----------------------------------------------------------------------------------------------------------------"
rospy.loginfo(info_str)
# shutdown setup process
rospy.signal_shutdown('Visual compass setup finished cleanly.')
def changed_config_param(self, config, param_name):
# type: (dict, str) -> bool
"""
TODO
"""
return param_name not in self.config or config[
param_name] != self.config[param_name]
class VisualCompassStartup():
# type: () -> None
"""
This node runs the visual compass in the game. It receives the current image and executes the visual compas worker.
In addition it loads the map describing the field background. In the end it executes some filters and publishes the results.
Subscribes to raw image
Publish: 'visual_compass'-messages
Returns angle, confidence
"""
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_startup')
rospy.loginfo('Initializing visual compass startup')
self.bridge = CvBridge()
self.config = {}
self.compass = None
self.orientation_offset = 0 # Orientation changes about PI in the second game half
self.filter = None
self.lastTimestamp = rospy.Time.now()
# Register publisher of 'visual_compass'-messages
self.pub_compass = rospy.Publisher('visual_compass',
PoseWithCertaintyStamped,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.spin()
def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
Dynamic reconfigure callback that sets a new configuration
"""
self.compass = VisualCompass(config)
feature_map, meta_data = self.load_feature_map(
config['feature_map_file_path'])
self.compass.set_feature_map(feature_map)
self.compass.set_mean_feature_count(meta_data['mean_feature_count'])
self.filter = VisualCompassFilter()
if self.changed_config_param(config, 'feature_map_file_path'):
self.is_feature_map_set = False
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_map_image_count'):
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | map images: %(feature_map_count)d'
% {
'type': config['compass_type'],
'matcher': config['compass_matcher'],
'feature_map_count':
config['compass_multiple_map_image_count']
})
# Subscribe to game state
self.game_state_msg = rospy.Subscriber('gamestate',
GameState,
self.gamestate_callback,
queue_size=1,
tcp_nodelay=True)
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
self.config = config
return self.config
def image_callback(self, image_msg):
# type: (Image) -> None
"""
Callback that receives the current image and runs the calculation.
"""
# Drops old images
# TODO: fix
# image_age = rospy.get_rostime() - image_msg.header.stamp
# if image_age.to_sec() > 0.1:
# print("Visual Compass: Dropped Image-message") # TODO debug printer
# return
now = rospy.Time.now()
if rospy.Duration(1) < now - self.lastTimestamp:
self.handle_image(image_msg)
self.lastTimestamp = rospy.Time.now()
def handle_image(self, image_msg):
# type: (Image) -> None
"""
Runs the visual compass worker and filter.
"""
image = self.bridge.imgmsg_to_cv2(image_msg, 'bgr8')
# Set image
compass_result_angle, compass_result_confidence = self.compass.process_image(
image)
# Filter results
result = self.filter.filterMeasurement(compass_result_angle,
compass_result_confidence,
image_msg.header.stamp)
# Publishes the 'visual_compass'-message
self.publish_rotation("base_footprint", image_msg.header.stamp,
result[0], result[1])
def gamestate_callback(self, msg):
"""
Recives the game state to determin which side is ours.
"""
if msg.firstHalf:
self.orientation_offset = 0
else:
self.orientation_offset = math.pi
def publish_rotation(self, header_frame_id, header_stamp, orientation,
confidence):
# type: (TODO, TODO, float, float) -> None
"""
Builds the ros message and publishes the result.
"""
msg = PoseWithCertaintyStamped()
# Create PoseWithCertaintyStamped-message where only the orentation is used
msg.header.frame_id = header_frame_id
msg.header.stamp = header_stamp
msg.pose.pose.pose.orientation = quaternion_from_euler(
0, 0, (orientation + self.orientation_offset) %
(2 *
math.pi)) # Orientation changes about PI in the second game half
msg.pose.confidence = confidence
# Publish VisualCompassMsg-message
self.pub_compass.publish(msg)
def load_feature_map(self, feature_map_file_path):
# type: (str) -> ([], [])
"""
Loads the map describing the surrounding field background
"""
# generate file path
file_path = self.package_path + feature_map_file_path
features = ([], [])
if path.isfile(file_path):
# load keypoints of pickle file
with open(file_path, 'rb') as f:
features = pickle.load(f)
rospy.loginfo('Loaded map file at: %(path)s' % {'path': file_path})
keypoint_values = features['keypoint_values']
descriptors = features['descriptors']
meta = features['meta']
# TODO broken
# self.check_meta_information(meta)
# convert keypoint values to cv2 Keypoints
keypoints = [
KeyPoint(kp[0], kp[1], kp[2], kp[3], kp[4], kp[5], kp[6])
for kp in keypoint_values
]
return ((keypoints, descriptors), meta)
else:
rospy.logerr('NO map file found at: %(path)s' %
{'path': file_path})
def check_meta_information(self, meta):
# type: (dict) -> None
"""
Ensures that the loaded map is compatible with the selected compass.
:param meta: meta information of the map
"""
rospy.loginfo(
'The map file was recorded at field %(field)a at date %(date)s on device %(device)'
% {
'field': meta['field'],
'date': meta['date'],
'device': meta['device']
})
if meta['keypoint_count'] != meta['descriptor_count']:
rospy.logerr(
'Number of keypoints does not match number of descriptors in map file.'
)
elif meta['compass_type'] != self.config['compass_type']:
rospy.logwarn('Config parameter "compass_type" does not match type in map:\n' + \
'config: %(config)s | map: %(map)a' % {'config': self.config['compass_type'], 'map': meta['compass_type']})
elif meta['compass_matcher'] != self.config['compass_matcher']:
rospy.logwarn('Config parameter "compass_compass" does not match parameter in map:\n' + \
'config: %(config)s | map: %(map)a' % {'config': self.config['compass_matcher'], 'map': meta['compass_matcher']})
elif meta['compass_multiple_map_image_count'] != self.config[
'compass_multiple_map_image_count']:
rospy.logwarn('Config parameter "compass_multiple_map_image_count" does not match parameter in map:\n' + \
'config: %(config)s | map: %(gt)a' % {'config': self.config['compass_multiple_map_image_count'], 'gt': meta['compass_multiple_map_image_count']})
def changed_config_param(self, config, param_name):
# type: (dict, str) -> bool
"""
TODO
"""
return param_name not in self.config or config[
param_name] != self.config[param_name]
class BinaryEvaluator(object):
def __init__(self, dimensions, angle_steps):
self.dimensions = dimensions
self.angle_steps = angle_steps
dirname = os.path.dirname(__file__)
relative_config_path = "../config/config.yaml"
config_path = os.path.join(dirname, relative_config_path)
with open(config_path, 'r') as stream:
config = yaml.load(stream)
relative_data_path = config['evaluation_data']
self.data_path = os.path.join(dirname, relative_data_path)
self.loader = DataLoader(self.data_path, self.dimensions, self.angle_steps)
config['compass_type'] = 'multiple'
self.sample_count = 2 if config['compass_type'] == 'binary' else config['compass_multiple_ground_truth_images_count']
self.vc = VisualCompass(config)
def show_img(self, image):
cv2.imshow("Record", image)
k = cv2.waitKey(1)
def evaluate(self):
self.setTruth()
self.evaluateAllImages()
def setTruth(self):
for i in range(self.sample_count):
angle = float(i) / self.sample_count * math.radians(360)
image = self.loader.get_image(4, 3, self.float_mod(angle + math.radians(90), math.radians(360)))
self.vc.set_truth(angle, image)
self.show_img(image)
time.sleep(0.5)
cv2.destroyAllWindows()
def debug_image_callback(self, debug_image):
return
self.show_img(debug_image)
time.sleep(0.5)
def evaluateAllImages(self):
confidences = list()
fail = 0.0
unsave = 0.5
confidence_threshold = 0.4
for row in range(self.dimensions[0]):
confidences.append(list())
for checkpoint in range(self.dimensions[1]):
confidences[row].append(list())
fails = list()
for angle in [4,12]: #,12]:# range(self.angle_steps):
image = self.loader.get_image(row, checkpoint, float(angle) / 16 * 2 * math.pi)
# self.show_img(image)
ground_truth = float(angle - 4)/16*2*math.pi
compass_result = self.vc.process_image(image, debugCB=self.debug_image_callback)
confidence = compass_result[1]
fail_val = 0.0
if (abs(ground_truth - compass_result[0]) > 0.0001 and compass_result[1] > confidence_threshold):
print("Bad detection", compass_result[0], ground_truth)
fail += 1
fail_val = 1.0
if compass_result[1] <= confidence_threshold:
if abs(ground_truth - compass_result[0]) > 0.0001:
print("Filtered false positive")
# self.show_img(image)
unsave += 1
print("Confidence too low. Value: {}".format(confidence))
confidence = 0.0
confidences[row][checkpoint].append(confidence)
fails.append(fail_val)
for fail_obj in fails:
confidences[row][checkpoint].append(fail_obj)
count = (self.dimensions[0] * self.dimensions[1]) * 2
plt.title('Binary evaluation')
plt.subplot(2, 2, 1)
plt.title('Confidence values torwards goal 1')
self.plot_confidence(confidences,0)
plt.subplot(2, 2, 2)
plt.title('Confidence values torwards goal 2')
self.plot_confidence(confidences,1)
plt.subplot(2, 2, 3)
plt.title('False positives torwards goal 1')
self.plot_confidence(confidences,2)
plt.subplot(2, 2, 4)
plt.title('False positives torwards goal 2')
self.plot_confidence(confidences,3)
plt.show()
print("Fail {}%".format(fail/count*100))
print("Unsave {}%".format(unsave/count*100))
def plot_confidence(self, confidences, side):
a = np.zeros((10, 7))
for row_index, row in enumerate(confidences):
for checkpoint_index, checkpoint in enumerate(row):
a[row_index, checkpoint_index] = float(checkpoint[side])
plt.imshow(a, cmap='hot', interpolation='nearest')
def float_mod(self, number, modulo):
return number - modulo * math.floor(number / modulo)
class VisualCompassStartup():
# type: () -> None
"""
TODO docs
Subscribes to raw image
Trigger: 'trigger_visual_compass'-trigger
Gets triggered e.i. while looking at a goal side
Returns side
Publish: 'visual_compass'-messages
Returns side
"""
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_startup')
rospy.loginfo('Initializing visual compass startup')
self.bridge = CvBridge()
self.config = {}
self.compass = None
self.orientation_offset = 0 # Orientation changes about PI in the second game half
self.filter = None
self.lastTimestamp = rospy.Time.now()
# Register publisher of 'visual_compass'-messages
self.pub_compass = rospy.Publisher('visual_compass',
VisualCompassRotation,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.spin()
def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
TODO docs
"""
self.compass = VisualCompass(config)
self.compass.set_ground_truth_features(
self.load_ground_truth(config['ground_truth_file_path']))
self.filter = VisualCompassFilter()
if self.changed_config_param(config, 'ground_truth_file_path'):
self.is_ground_truth_set = False
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_ground_truth_images_count'):
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | ground truth images: %(ground_truth_count)d'
% {
'type':
config['compass_type'],
'matcher':
config['compass_matcher'],
'ground_truth_count':
config['compass_multiple_ground_truth_images_count']
})
# Subscribe to game state
self.game_state_msg = rospy.Subscriber('gamestate',
GameState,
self.gamestate_callback,
queue_size=1,
tcp_nodelay=True)
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
self.config = config
return self.config
def image_callback(self, image_msg):
# type: (Image) -> None
"""
TODO docs
"""
# Drops old images
# TODO: fix
# image_age = rospy.get_rostime() - image_msg.header.stamp
# if image_age.to_sec() > 0.1:
# print("Visual Compass: Dropped Image-message") # TODO debug printer
# return
now = rospy.Time.now()
if rospy.Duration(1) < now - self.lastTimestamp:
self.handle_image(image_msg)
self.lastTimestamp = rospy.Time.now()
def handle_image(self, image_msg):
# type: (Image) -> None
"""
TODO docs
"""
image = self.bridge.imgmsg_to_cv2(image_msg, 'bgr8')
# Set image
compass_result_angle, compass_result_confidence = self.compass.process_image(
image)
# Filter results
result = self.filter.filterMeasurement(compass_result_angle,
compass_result_confidence,
image_msg.header.stamp)
# Publishes the 'visual_compass'-message
self.publish_rotation("base_footprint", image_msg.header.stamp,
result[0], result[1])
def gamestate_callback(self, msg):
if msg.firstHalf:
self.orientation_offset = 0
else:
self.orientation_offset = math.pi
def publish_rotation(self, header_frame_id, header_stamp, orientation,
confidence):
# type: (TODO, TODO, float, float) -> None
"""
TODO docs
"""
msg = VisualCompassRotation()
# Create VisualCompassRotation-message
msg.header.frame_id = header_frame_id
msg.header.stamp = header_stamp
msg.orientation = (orientation + self.orientation_offset) % (
2 * math.pi
) # Orientation changes about PI in the second game half
msg.confidence = confidence
# Publish VisualCompassMsg-message
self.pub_compass.publish(msg)
def load_ground_truth(self, ground_truth_file_path):
# type: (str) -> ([], [])
"""
TODO docs
"""
# generate file path
file_path = self.package_path + ground_truth_file_path
features = ([], [])
if path.isfile(file_path):
# load keypoints of pickle file
with open(file_path, 'rb') as f:
features = pickle.load(f)
rospy.loginfo('Loaded ground truth file at: %(path)s' %
{'path': file_path})
keypoint_values = features['keypoint_values']
descriptors = features['descriptors']
meta = features['meta']
# TODO broken
# self.check_meta_information(meta)
# convert keypoint values to cv2 Keypoints
keypoints = [
KeyPoint(kp[0], kp[1], kp[2], kp[3], kp[4], kp[5], kp[6])
for kp in keypoint_values
]
return (keypoints, descriptors)
else:
rospy.logerr('NO ground truth file found at: %(path)s' %
{'path': file_path})
def check_meta_information(self, meta):
# type: (dict) -> None
"""
TODO docs
"""
rospy.loginfo(
'The ground truth file was recorded at field %(field)a at date %(date)s on device %(device)'
% {
'field': meta['field'],
'date': meta['date'],
'device': meta['device']
})
if meta['keypoint_count'] != meta['descriptor_count']:
rospy.logerr(
'Number of keypoints does not match number of descriptors in ground truth file.'
)
elif meta['compass_type'] != self.config['compass_type']:
rospy.logwarn('Config parameter "compass_type" does not match ground truth:\n' + \
'config: %(config)s | ground truth: %(gt)a' % {'config': self.config['compass_type'], 'gt': meta['compass_type']})
elif meta['compass_matcher'] != self.config['compass_matcher']:
rospy.logwarn('Config parameter "compass_compass" does not match ground truth:\n' + \
'config: %(config)s | ground truth: %(gt)a' % {'config': self.config['compass_matcher'], 'gt': meta['compass_matcher']})
elif meta['compass_multiple_ground_truth_images_count'] != self.config[
'compass_multiple_ground_truth_images_count']:
rospy.logwarn('Config parameter "compass_multiple_ground_truth_images_count" does not match ground truth:\n' + \
'config: %(config)s | ground truth: %(gt)a' % {'config': self.config['compass_multiple_ground_truth_images_count'], 'gt': meta['compass_multiple_ground_truth_images_count']})
def changed_config_param(self, config, param_name):
# type: (dict, str) -> bool
"""
TODO
"""
return param_name not in self.config or config[
param_name] != self.config[param_name]