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)