def get_robot_pos_drawing(robot_positions,
colors,
circle_radius=10,
arrow_thickness=4):
field_drawing, field_drawing_scale = draw_field_key_points(
[],
500,
3,
10,
get_key_points_top_down_field(FieldDimensions()), [],
draw_key_points=False)
if robot_positions is None or colors is None:
return field_drawing
for robot_pos, color in zip(robot_positions, colors):
tx = robot_pos.x * field_drawing_scale
ty = robot_pos.y * field_drawing_scale
cv2.circle(field_drawing, (int(tx), int(ty)), circle_radius, color, -1)
look_at = [
math.cos(
(-robot_pos.rot_left_right_torso + 90) / 180 * np.pi) * 30 +
tx,
math.sin(
(-robot_pos.rot_left_right_torso + 90) / 180 * np.pi) * 30 + ty
]
cv2.arrowedLine(field_drawing, (int(tx), int(ty)),
(int(look_at[0]), int(look_at[1])),
color,
thickness=arrow_thickness,
tipLength=0.4)
return field_drawing
def get_position_of_mask_with_keypoints_and_search(annotations, loss, num_of_optimizing_steps, batchsize, rendered_line_width, verbose=0):
mask = util.get_mask_segmentation(annotations, 640, 480)
init_robot_pos = frp_keypoints.get_robot_position_with_key_points(mask, verbose)
if init_robot_pos is None:
fd = FieldDimensions()
init_robot_pos = robot_position.RobotPosition(
x=fd.border_strip_width + fd.field_x*2/4, y=fd.border_strip_width + fd.field_y*3/4, rot_left_right_torso=180, rot_left_right_head=0, rot_top_down=65, rot_clockwise=0)
domain = {
"x": [-fd.field_x/2, fd.field_x/2],
"y": [-fd.field_y/4, fd.field_y/4],
"rot_left_right_torso":[-180, 180],
"rot_left_right_head":0,
"rot_top_down":[-25, 25],
"rot_clockwise":0
}
else:
domain = {
"x": [-1000, 1000],
"y": [-1000, 1000],
"rot_left_right_torso":[-30, 30],
"rot_left_right_head":0,
"rot_top_down":[-15, 15],
"rot_clockwise":0
}
search_robot_positions_and_loss = frp_renderer.get_robot_position_optimize_renderer(
mask, init_robot_pos, domain, loss, num_of_optimizing_steps, batchsize, rendered_line_width, verbose)
best_robot_pos = search_robot_positions_and_loss[0,0]
best_loss = search_robot_positions_and_loss[0,1]
if verbose > 0:
class_id_to_color = [[0, 0, 0], [255,0,0], [0,255,0], [0,0,255], [122,122,0], [0,122,122], [122,0,122], [255,122,122], [122,255,122], [122,122,255]]
init_robot_view = render_robot_position.get_robot_position_rendered_rgb_image(init_robot_pos, 110)
init_robot_diff = render_robot_position.get_diff_visualisation(mask, render_robot_position.rgb_mask_to_label_mask(init_robot_view, class_id_to_color))
best_robot_view = render_robot_position.get_robot_position_rendered_rgb_image(best_robot_pos, 110)
best_robot_diff = render_robot_position.get_diff_visualisation(mask, render_robot_position.rgb_mask_to_label_mask(best_robot_view, class_id_to_color))
img_array = [
["cmask", util.get_colored_segmentation_mask(mask, class_id_to_color)],
["cmask", util.get_colored_segmentation_mask(mask, class_id_to_color)],
["search_space", frp_renderer.get_robot_pos_drawing_from_search_result(search_robot_positions_and_loss)],
["init_robot_view", init_robot_view],
["init_robot_diff", util.get_colored_segmentation_mask(init_robot_diff, class_id_to_color)],
["init_robot_top_view", render_robot_position.get_robot_pos_drawing([init_robot_pos], [[0, 0, 255]])],
["best_robot_view", best_robot_view],
["best_robot_diff", util.get_colored_segmentation_mask(best_robot_diff, class_id_to_color)],
["best_robot_top_view", render_robot_position.get_robot_pos_drawing([best_robot_pos], [[0, 0, 255]])],
]
fig = util.get_figure_of_images(img_array, 3, 3, 10)
return best_robot_pos, best_loss, fig
return best_robot_pos, best_loss
def show_keypoint_and_view(kp_img_drawing, key_points_img):
kp_field = get_key_points_top_down_field(FieldDimensions())
kp_img_list, kp_field_list = get_keypoint_list(key_points_img, kp_field)
h, status = cv2.findHomography(kp_img_list, kp_field_list)
robot_pos, view_points_in_mask, view_points_in_field_in_mm, look_at_point_in_mm = get_robot_position_from_homography_matrix(
h)
kp_colors = [ran_color(30) for i in range(len(kp_img_list))]
draw_key_points_on_img(kp_img_drawing, kp_img_list, 9, kp_colors)
kp_field_drawing, field_drawing_scale = draw_field_key_points(
kp_field_list, 500, 3, 10, kp_field, kp_colors)
view_points_in_field = np.array(view_points_in_field_in_mm *
field_drawing_scale)
robot_pos_in_drawing = [
robot_pos.x * field_drawing_scale, robot_pos.y * field_drawing_scale
]
cv2.polylines(kp_img_drawing, np.int32([view_points_in_mask]), 1,
(50, 100, 255), 4)
cv2.polylines(kp_field_drawing, np.int32([view_points_in_field]), 1,
(50, 100, 255), 2)
cv2.circle(kp_field_drawing,
(int(robot_pos_in_drawing[0]), int(robot_pos_in_drawing[1])),
10, (50, 100, 255), -1)
cv2.circle(kp_field_drawing,
(int(look_at_point_in_mm[0] * field_drawing_scale),
int(look_at_point_in_mm[1] * field_drawing_scale)), 10,
(50, 100, 255), -1)
sight_vector_left = (view_points_in_field[0, :] - view_points_in_field[
1, :]) * 100 + robot_pos_in_drawing
sight_vector_right = (view_points_in_field[3, :] - view_points_in_field[
2, :]) * 100 + robot_pos_in_drawing
cv2.line(kp_field_drawing,
(int(robot_pos_in_drawing[0]), int(robot_pos_in_drawing[1])),
(int(sight_vector_left[0]), int(sight_vector_left[1])),
(50, 100, 255), 2)
cv2.line(kp_field_drawing,
(int(robot_pos_in_drawing[0]), int(robot_pos_in_drawing[1])),
(int(sight_vector_right[0]), int(sight_vector_right[1])),
(50, 100, 255), 2)
img_array = [["kp_img_drawing", kp_img_drawing],
["kp_field_drawing", kp_field_drawing]]
fig = util.get_figure_of_images(img_array, 2, 1, 10)
def get_random_plausible_robot_position(verbose=0):
# add the argument to specify how many pixel of each label is needed for the images to be plausible
fd = FieldDimensions()
x = np.random.randint(fd.border_strip_width,
fd.size_x - fd.border_strip_width)
y = np.random.randint(fd.mid_y, fd.size_y - fd.border_strip_width)
rot_left_right_torso = np.random.randint(0, 360)
rot_left_right_head = 0
rot_top_down = np.random.randint(55, 90)
rot_clockwise = 0
pos = robot_position.RobotPosition(x, y, rot_left_right_torso,
rot_left_right_head, rot_top_down,
rot_clockwise)
rendered_rgb_pos = render_robot_position.get_robot_position_rendered_rgb_image(
pos, 110)
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122]]
rendered_label_mask = render_robot_position.rgb_mask_to_label_mask(
rendered_rgb_pos, class_id_to_color)
rendered_rgb_pos_from_mask = util.get_colored_segmentation_mask(
rendered_label_mask, class_id_to_color)
unique_label, label_count = np.unique(rendered_label_mask,
return_counts=True)
if verbose > 1:
print(unique_label, label_count)
if 1 in unique_label and label_count[
1] > 100 and 2 in unique_label and label_count[
2] > 100 and 3 in unique_label and label_count[3] > 100:
if verbose > 0:
util.get_figure_of_images(
[["rendered_rgb_pos_from_mask", rendered_rgb_pos_from_mask]],
1, 1, 10)
return pos, rendered_label_mask
else:
return get_random_plausible_robot_position(verbose)
def get_robot_position_with_key_points(mask, verbose):
rotated_rect_ellipse = get_ellipse(mask, verbose > 3)
line_cluster = get_line_cluster(mask, verbose > 3)
penalty_cross_center = get_penalty_cross_center(mask)
goal_bbox = get_goal_bbox(mask)
key_points_img = get_key_points_mask(rotated_rect_ellipse, line_cluster,
penalty_cross_center, goal_bbox)
if len(key_points_img) < 4:
return None
kp_field = get_key_points_top_down_field(FieldDimensions())
kp_img_list, kp_field_list = get_keypoint_list(key_points_img, kp_field)
h, status = cv2.findHomography(kp_img_list, kp_field_list)
robot_pos = get_robot_position_from_homography_matrix(h)[0]
if verbose > 2:
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122]]
cmask = util.get_colored_segmentation_mask(mask, class_id_to_color)
show_keypoint_and_view(
get_concept_img(cmask, rotated_rect_ellipse, line_cluster,
penalty_cross_center, goal_bbox), key_points_img)
return robot_pos
def get_robot_position_optimize_renderer(gt_label_mask,
init_robot_position,
domain,
loss_func,
num_of_optimizing_steps,
batchsize,
rendered_line_width=110,
verbose=0):
# restrict the domain if it is not in the field
fd = FieldDimensions()
if isinstance(domain["y"], list):
domain["y"][1] = np.min([
domain["y"][1],
fd.border_strip_width + fd.field_y - init_robot_position.y
])
if isinstance(domain["x"], list):
domain["x"][0] = np.max(
[domain["x"][0], fd.border_strip_width - init_robot_position.x])
domain["x"][1] = np.min([
domain["x"][1],
fd.border_strip_width + fd.field_x - init_robot_position.x
])
# create the domain for the fitting function
par_to_key = []
domain_array = []
robo_pos = {}
for key, value in domain.items():
if isinstance(value, list):
par_to_key.append(key)
domain_array.append(value)
else:
robo_pos[key] = value
def function_to_optimize(par):
for i, key in enumerate(par_to_key):
robo_pos[key] = par[i]
diff_robot_pos = RobotPosition(robo_pos["x"], robo_pos["y"],
robo_pos["rot_left_right_torso"],
robo_pos["rot_left_right_head"],
robo_pos["rot_top_down"],
robo_pos["rot_clockwise"])
robot_pos = add_robot_postions(init_robot_position, diff_robot_pos)
rendered_rgb_image = get_robot_position_rendered_rgb_image(
robot_pos, rendered_line_width)
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122]]
rendered_label_mask = rgb_mask_to_label_mask(rendered_rgb_image,
class_id_to_color)
loss = loss_func(gt_label_mask, rendered_label_mask)
return loss
init_loss = function_to_optimize(np.zeros(len(domain)))
if verbose > 1:
print("init_robot_position loss:", init_loss)
start = default_timer()
bb.search_min(f=function_to_optimize,
domain=domain_array,
budget=num_of_optimizing_steps,
batch=batchsize,
resfile='output.csv',
executor=CustomExecutor)
end = default_timer()
if verbose > 1:
print("calculation with {} steps took {:.2f}s and {:.2f}s/step".format(
num_of_optimizing_steps, end - start,
(end - start) / num_of_optimizing_steps))
search_result = pd.read_csv('output.csv').values
os.remove('output.csv')
search_robot_positions_and_loss = []
for res_row in search_result:
for i, key in enumerate(par_to_key):
robo_pos[key] = res_row[i]
diff_robot_pos = RobotPosition(robo_pos["x"], robo_pos["y"],
robo_pos["rot_left_right_torso"],
robo_pos["rot_left_right_head"],
robo_pos["rot_top_down"],
robo_pos["rot_clockwise"])
temp_robot_pos = add_robot_postions(init_robot_position,
diff_robot_pos)
search_robot_positions_and_loss.append([temp_robot_pos, res_row[-1]])
search_robot_positions_and_loss.append([init_robot_position, init_loss])
search_robot_positions_and_loss = np.array(search_robot_positions_and_loss)
search_robot_positions_and_loss = search_robot_positions_and_loss[
np.argsort(search_robot_positions_and_loss[:, 1])]
if verbose > 1:
print("best loss at:", search_robot_positions_and_loss[0, 1])
return search_robot_positions_and_loss
def get_robot_position_optimize_renderer_grid_search(gt_label_mask,
domain,
loss_func,
rendered_line_width=110,
verbose=0):
fd = FieldDimensions()
init_robot_pos = RobotPosition(
x=fd.border_strip_width + fd.field_x * 2 / 4,
y=fd.border_strip_width + fd.field_y * 3 / 4,
rot_left_right_torso=180,
rot_left_right_head=0,
rot_top_down=65,
rot_clockwise=0)
domain_values = []
for _, value in domain.items():
if isinstance(value, list):
dx = (value[1] - value[0]) / (value[2])
domain_values.append(np.arange(value[0] + dx / 2, value[1], dx))
else:
domain_values.append(np.array([value]))
all_combinations = np.stack(np.meshgrid(*domain_values),
-1).reshape(-1, len(domain_values))
if verbose > 0:
print("there are ", len(all_combinations), "combinations")
results = []
for i, combination in enumerate(all_combinations):
diff_robot_pos = RobotPosition(*combination)
robot_pos = add_robot_postions(init_robot_pos, diff_robot_pos)
rendered_rgb_image = get_robot_position_rendered_rgb_image(
robot_pos, rendered_line_width)
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122]]
rendered_label_mask = rgb_mask_to_label_mask(rendered_rgb_image,
class_id_to_color)
loss = loss_func(gt_label_mask, rendered_label_mask)
results.append([robot_pos, loss])
if verbose > 0:
sys.stdout.write("\r" + str(i + 1) + " / " +
str(len(all_combinations)))
sys.stdout.flush()
if verbose > 0:
print("")
results = np.array(results)
results = results[np.argsort(results[:, 1])]
#search_robot_positions_and_loss = find_robot_pos_grid_search(gt_label_mask, verbose=1)
best_robot_pos = results[0, 0]
best_loss = results[0, 1]
if verbose > 1:
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122],
[255, 122, 122], [122, 255, 122], [122, 122, 255]]
best_robot_view = get_robot_position_rendered_rgb_image(
best_robot_pos, 110)
best_robot_diff = get_diff_visualisation(
gt_label_mask,
rgb_mask_to_label_mask(best_robot_view, class_id_to_color))
img_array = [
[
"cmask",
util.get_colored_segmentation_mask(gt_label_mask,
class_id_to_color)
],
[
"cmask",
util.get_colored_segmentation_mask(gt_label_mask,
class_id_to_color)
],
[
"search_space",
get_robot_pos_drawing_from_search_result(results)
],
["best_robot_view", best_robot_view],
[
"best_robot_diff",
util.get_colored_segmentation_mask(best_robot_diff,
class_id_to_color)
],
[
"best_robot_top_view",
get_robot_pos_drawing([best_robot_pos], [[0, 0, 255]])
],
]
util.get_figure_of_images(img_array, 3, 2, 7)
print("best loss:", best_loss)
return results
def draw_on_scene(scene, line_thickness):
fd = FieldDimensions()
class_id_to_color = [[0, 0, 0], [255, 0, 0], [0, 255, 0], [0, 0, 255],
[122, 122, 0], [0, 122, 122], [122, 0, 122]]
line_color = class_id_to_color[1]
center_circle_color = class_id_to_color[2]
goal_color = class_id_to_color[3]
penalty_cross_color = class_id_to_color[4]
draw_line(scene, [fd.mid_x, fd.mid_y], fd.field_x, True, line_color,
line_thickness)
draw_line(scene, [fd.border_strip_width, fd.mid_y], fd.field_y, False,
line_color, line_thickness)
draw_line(scene, [fd.size_x - fd.border_strip_width, fd.mid_y], fd.field_y,
False, line_color, line_thickness)
draw_line(scene, [fd.mid_x, fd.border_strip_width],
fd.field_x + line_thickness, True, line_color, line_thickness)
draw_line(scene, [fd.mid_x, fd.size_y - fd.border_strip_width],
fd.field_x + line_thickness, True, line_color, line_thickness)
draw_line(scene,
[fd.mid_x, fd.border_strip_width + fd.penalty_area_length],
fd.penalty_area_width + line_thickness, True, line_color,
line_thickness)
draw_line(scene, [
fd.mid_x - fd.penalty_area_width / 2,
fd.border_strip_width + fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_line(scene, [
fd.mid_x + fd.penalty_area_width / 2,
fd.border_strip_width + fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_line(
scene,
[fd.mid_x, fd.size_y - fd.border_strip_width - fd.penalty_area_length],
fd.penalty_area_width + line_thickness, True, line_color,
line_thickness)
draw_line(scene, [
fd.mid_x - fd.penalty_area_width / 2,
fd.size_y - fd.border_strip_width - fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_line(scene, [
fd.mid_x + fd.penalty_area_width / 2,
fd.size_y - fd.border_strip_width - fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_circle(scene, [fd.mid_x, fd.mid_y], fd.center_circle_diameter / 2,
center_circle_color, line_thickness)
draw_box(scene, [
fd.mid_x, fd.border_strip_width,
fd.goal_height / 2 + fd.goal_post_diameter / 2
], [
fd.goal_width + fd.goal_post_diameter, 0.1,
fd.goal_height + fd.goal_post_diameter
], goal_color)
draw_box(scene, [
fd.mid_x, fd.size_y - fd.border_strip_width,
fd.goal_height / 2 + fd.goal_post_diameter / 2
], [
fd.goal_width + fd.goal_post_diameter, 0.1,
fd.goal_height + fd.goal_post_diameter
], goal_color)
draw_line(scene, [
fd.mid_x - fd.penalty_area_width / 2,
fd.size_y - fd.border_strip_width - fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_line(scene, [
fd.mid_x + fd.penalty_area_width / 2,
fd.size_y - fd.border_strip_width - fd.penalty_area_length / 2
], fd.penalty_area_length, False, line_color, line_thickness)
draw_line(scene, [
fd.mid_x, fd.size_y - fd.border_strip_width - fd.penalty_cross_distance
], line_thickness * 1.25, False, line_color, line_thickness * 0.70)
draw_line(scene, [
fd.mid_x, fd.size_y - fd.border_strip_width - fd.penalty_cross_distance
], line_thickness * 1.25, True, line_color, line_thickness * 0.70)
draw_line(scene,
[fd.mid_x, fd.border_strip_width + fd.penalty_cross_distance],
line_thickness * 1.25, False, line_color, line_thickness * 0.70)
draw_line(scene,
[fd.mid_x, fd.border_strip_width + fd.penalty_cross_distance],
line_thickness * 1.25, True, line_color, line_thickness * 0.70)
return [fd.size_x, fd.size_y]