from triangulation import Camera, Triangulation, InsufficientDataException
import numpy as np
if __name__ == "__main__":
# Initialise cameras and enter normalized directional vector
camera1 = Camera(np.array([250, -275, 84]))
camera1.normalize_direction(np.array([-211.9, 225, 77, -84]))
camera2 = Camera(np.array([250, 275, 84]))
camera2.normalize_direction(np.array([-221.41, -223.62, -84]))
camera3 = Camera(np.array([-250, 275, 84]))
camera3.normalize_direction(np.array([203.077, -208.711, -84]))
camera4 = Camera(np.array([-250, -275, 84]))
camera4.normalize_direction(np.array([205.669, 216.506, -84]))
triang = Triangulation([camera1, camera2, camera3, camera4])
camera1.ball_pos = np.array([20, 60])
camera2.ball_pos = np.array([-75, 50])
camera3.ball_pos = np.array([48, 95])
camera4.ball_pos = np.array([-47, 82])
try:
position = triang.calculate_position()
print(position)
except InsufficientDataException:
print(
"Position could not be found because of insufficient picture data")
wait = input("Press enter to close")
current_cam.not_found = True
else:
coordinates = np.array([
coordinates_tuple[0] - res_x / 2,
(coordinates_tuple[1] - res_y / 2) * -1
])
current_cam.not_found = False
current_cam.ball_pos = coordinates
# Break out of the loop if no more frames are provided
if no_img:
break
# Calculate ball position from camera angles
try:
position = triangulation.calculate_position()
ball_curve.append(position)
position_list.append(position.tolist())
except InsufficientDataException:
# Fill curve with impossible position
ball_curve.append(np.array([[0], [0], [-10]]))
#for simulation use last good position
if len(position_list) <= 0:
position_list.append([0, 0, 0])
else:
position_list.append(position_list[-1])
frame_counter += 1
printProgressBar(frame_counter,
total_frames,
prefix="Progress:",
length=50)
