def main_pc():
# this code uses pictures taken manualy
# this is not the correct way to use the code but because of COVID-19, the setup was not obtainable
folder = 'D:\_Udes\S4\Projet\ScanUS\Photos_boite/'
files = os.listdir(folder)
laser_angles = [-5, -3, -2, -1, -0.5, 0, 0.5, 1.5, 2, 3, 4]
my_plot = plot.Plot(name='main plot', range=[0, 10])
position_laser_ref_plaque = Matrix(pos=[259, 512, 150])
angle_laser_cam = 11.1 * 2 * math.pi / 360 + math.atan(259 / 512)
trans_plaque_to_cam_ref = Matrix(angles=[0, 0, angle_laser_cam])
position_laser = mult([trans_plaque_to_cam_ref, position_laser_ref_plaque])
for i in range(len(files)):
filename = folder + files[i]
img = cv2.imread(filename)
x, y, fail = camera.find_red_dot(frame=img)
if not fail:
angle_table = ((i * 11.25) % 360) * 2 * math.pi / 360
angle_laser = laser_angles[i // 32] * 2 * math.pi / 360
p1, v1 = camera.get_red_dot_point_vector_in_world(
angle_table, x, y)
p2, v2 = laser.get_laser_point_vector_in_world(
angle_table=angle_table, angle_wrist=angle_laser)
p, error = intersect(p1, v1, p2, v2)
my_plot.add_point(p + [error])
if i > 11 * 32:
break
input()
my_plot.close()
def test_mult(self):
# expected answers where hand calculated
ma = Matrix(pos=[1, 2, 3], angles=[0, -pi / 2, 0])
mb = Matrix(pos=[0, 0, 0], angles=[0, 0, pi / 2])
expected_a_b = np.array([[0, 0, -1, 1], [1, 0, 0, 2], [0, -1, 0, 3],
[0, 0, 0, 1]])
expected_b_a = np.array([[0, -1, 0, -2], [0, 0, -1, 1], [1, 0, 0, 3],
[0, 0, 0, 1]])
np.testing.assert_allclose(mult([ma, mb]).matrix,
expected_a_b,
atol=0.01)
np.testing.assert_allclose(mult([mb, ma]).matrix,
expected_b_a,
atol=0.01)
def get_camera_ext_matrix(angle_table=0,
cam_pos=CAMERA_POS,
cam_angles=CAMERA_ANGLES):
# The camera ext matrix should have its coordinate system with
# the y axis pointing through the center of the image
floor_matrix = Matrix(angles=[0, 0, angle_table])
cam_matrix = Matrix(pos=cam_pos, angles=cam_angles)
result_matrix = mult([floor_matrix, cam_matrix])
return result_matrix
def get_laser_point_vector_in_world(angle_table=0,
angle_tower=0,
angle_wrist=0):
laser_matrix_world = get_laser_line_in_world(angle_table, angle_tower,
angle_wrist)
point = laser_matrix_world.get_pos()
unit_vector_matrix = Matrix(pos=[0, 1, 0])
point_vector_direction = mult([laser_matrix_world, unit_vector_matrix])
vector = point_vector_direction.get_pos() - point
return point, vector
def get_red_dot_point_vector_in_world(angle_table=0,
x_image=0,
y_image=0,
cam_pos=CAMERA_POS,
cam_angles=CAMERA_ANGLES):
m_cam_ext = get_camera_ext_matrix(angle_table, cam_pos, cam_angles)
point = m_cam_ext.get_pos()
v_red_dot_cam_ref = get_red_dot_vector_from_cam(x_image, y_image)
m_red_dot_cam_ref = Matrix(pos=v_red_dot_cam_ref)
m_red_dot_world_ref = mult([m_cam_ext, m_red_dot_cam_ref])
p_red_dot_world_ref = m_red_dot_world_ref.get_pos()
p_cam_world_ref = m_cam_ext.get_pos()
vector = list(p_red_dot_world_ref - p_cam_world_ref)
return point, vector
def main_pi():
# This code is obsolete
# This code needs to be changed with functions from laser.py if the actual laser tower is used
my_plot = plot.Plot(name='main plot', range=[0, 10])
laser_p = [0, 0, 180]
with camera.init_picamera() as cam:
input()
motor.start_motor()
temp = 0
while (True):
pic = camera.take_one_picture_pi(cam)
# print(pic)
ang = -motor.get_angle_motor()
x, y = camera.find_red_dot(pic)
if ang > temp:
break
floor_matrix = Matrix(angles=[0, 0, ang])
cam_matrix = Matrix(pos=laser_p, angles=[0, 0, 0.26])
result_matrix = mult([floor_matrix, cam_matrix])
p2 = result_matrix.get_pos()
v2 = result_matrix.get_vector_in_referential([0, 1, 0])
p1, v1 = camera.get_red_dot_point_vector_in_world(ang, x, y)
m, l = intersect(p1, v1, p2, v2)
point = m + [l]
my_plot.add_point(point)
# print(m, l, sep=' ')
temp = ang
# k = input()
# if k == 'q':
# break
motor.restart_motor()
input()
my_plot.close()
def get_laser_line_in_world(angle_table=0, angle_tower=0, angle_wrist=0):
floor_matrix = Matrix(angles=[0, 0, angle_table])
wrist_matrix = get_wrist_matrix(angle_wrist)
tower_matrix = get_tower_matrix(angle_tower)
laser_matrix = mult([floor_matrix, tower_matrix, wrist_matrix])
return laser_matrix
def get_tower_matrix(angle_tower=0):
height = angle_to_dist(angle_tower)
fixed_matrix = Matrix(pos=TOWER_POS, angles=TOWER_ANGLES)
variable_matrix = Matrix(pos=[0, 0, height])
return mult([fixed_matrix, variable_matrix])
def get_wrist_matrix(angle_wrist=0):
fixed_matrix = Matrix(pos=WRIST_POS, angles=WRIST_ANGLES)
variable_matrix = Matrix(angles=[angle_wrist, 0, 0])
return mult([fixed_matrix, variable_matrix])