cost_c = cost(current, detections)
cost_f = cost(following, detections) * 1.25
# Find best match
costs = sorted([(current, cost_c), (following, cost_f)],
key=lambda x: x[1])
logging.info("Cost Current: %f", cost_c)
logging.info("Cost Following: %f", cost_f)
current_location = costs[0][0]
logging.info("New location: %s", current_location)
t_e = time.time()
time_data.append(t_e - t_s)
t_s = time.time()
fig = mr.show_route(current_location.node_id)
img = render_result(img, fig)
t_e = time.time()
time_data.append(t_e - t_s)
fps_stop = time.time()
logging.warning("Fps: %f", 1 / (fps_stop - fps_start))
logging.warning("--------------------------------")
time_data.append(fps_stop - fps_start)
output_file.write("{},{}\n".format(image_path, current_location.id))
fps_file.write('{}\n'.format(','.join(map(str, time_data))))
cv2.waitKey(1)
