cv2.circle(image, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
if show_lights == True:
text = str(round(
light_camera_coordinates[idx][0][0], 1)) + "," + str(
round(light_camera_coordinates[idx][1][0], 1))
cv2.putText(image, text, (int(p[0]) - 1, int(p[1]) - 1),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2,
cv2.LINE_AA)
print("Euler angles ", euler_angles)
print("Translation angles", translation_vector)
heading = np.degrees(
np.arctan2(translation_vector[0][0], translation_vector[2][0]))
elevation = np.degrees(
np.arctan2(translation_vector[1][0], translation_vector[2][0]))
m1.fill_target_heading_elevation(translation_vector, heading,
elevation)
m2.fill_light_positions(light_camera_coordinates)
text = "position in cm:" + str(round(
translation_vector[0][0], 1)) + "," + str(
round(translation_vector[1][0], 1)) + "," + str(
round(translation_vector[2][0], 1))
cv2.putText(image, text, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
(255, 0, 0), 2, cv2.LINE_AA)
text = "distance,heading,elevation in m,deg,deg:" + str(
round(np.linalg.norm(translation_vector) / 100,
2)) + "," + str(round(heading, 1)) + "," + str(
round(elevation, 1))
cv2.putText(image, text, (20, image.shape[0] - 70),
